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Abstract 

This  dissertation  is  concerned  with  processing  of  visual  motion  with  application  to  oflF-road 
vehicular  navigation.  Several  aspects  of  the  problem  are  investigated.  First,  we  consider  the 
estimation  of  total  rotation  from  a  sequence.  This  procedure  is  important  for  motion  analysis, 
image  stabilization,  and  independently  moving  object  detection,  as  well  as  the  recovery  of  other 
structural  information.  We  exploit  the  dynamic  nature  of  a  sequence  and  use  multiple  visual  cues  to 
perform  image  stabilization.  Depending  on  the  knowledge  of  intrinsic  parameters  such  as  the  focal 
length  and  the  field  of  view,  both  calibrated  and  uncalibrated  stabilization  schemes  are  designed. 
The  residual  motion  in  a  stabilized  sequence  is  also  analyzed.  Next  we  address  the  issue  of  selective 
stabilization,  defined  as  the  separation  of  the  smooth  rotation  and  the  residual  oscillatory  rotation. 
In  off-road  vehicular  navigation,  in  addition  to  smooth  motion,  a  vehicle  exhibits  residual  vibrations. 
These  residual  oscillatory  components  often  affect  the  interpretation  of  visual  information.  We 
incorporate  a  kinetic  model  to  explicitly  account  for  vibration  phenomena.  A  maneuver  detection 
scheme,  for  detecting  the  beginning  and  end  of  smooth  rotation,  is  designed  to  facilitate  the  selective 
stabilization.  The  structure  parameters  are  consequently  estimated  in  a  less  perturbed  frame  of 
reference.  Finally,  we  study  the  problem  of  feature  correspondence.  Tracking  feature  points  has 
been  a  critical  procedure  in  exploiting  an  image  sequence.  We  propose  a  localized  feature  point 
tracking  algorithm.  The  method  employs  a  2D  kinematic  model  and  relies  on  a  Probabilistic  Data 
Association  Filter  for  the  estimation  of  inter-frame  motion.  Corresponding  points  are  identified 
to  sub-pixel  accuracy  and  an  Extended  Kalman  Filter  is  employed  to  process  the  new  data.  The 
ability  to  dynamically  include  new  feature  points  from  subsequent  frames  also  makes  the  algorithm 
suitable  for  structure  from  motion  and  tracking  over  a  sequence. 
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Chapter  1 


Introduction 


1.1  Motivations 

Vision  plays  an  important  role  in  the  successful  operation  of  intelligent  mobile  robots.  In  order  to 
interact  with  its  environment,  it  is  essential  that  the  robot  have  the  ability  to  process  dynamic  im¬ 
agery  for  the  recovery  of  its  own  motion  and  relevant  environmental  structure.  Other  related  tasks 
such  as  moving  object  detection,  obstacle  avoidance,  target  acquisition,  etc.,  can  be  subsequently 
accomplished. 

The  interpretation  and  analysis  of  visual  motion  has  been  an  active  area  of  research  for  over 
two  decades.  Depending  on  the  image  motion  magnitude,  either  a  continuous  or  a  discrete 
form  is  employed  to  describe  image  motion;  continuous  description  leads  to  the  computation  of 
dense  motion  fields,  while  the  discrete  characterization  yields  sparse  feature  point  displacements. 
Accordingly,  major  techniques  for  motion/structure  analysis  fall  into  either  the  flow-based  or  the 
feature-based  domain. 

Flow-based  methods  employ  spatial  and  temporal  image  gradients  to  infer  motion  fields  and 
then  seek  out  the  interpretation  of  motion/structures.  Two  basic  assumptions  are  involved  here. 
One  is  the  so-called  the  constant  brightness  constraint:  the  image  brightnesses  of  pixels  correspond¬ 
ing  to  the  same  3D  points  remain  constant.  The  other  assumption  is  that  the  computed  optical 
flow  is  identical  to  the  motion  field — the  projection  of  the  3D  velocity  vector  onto  the  image  plane. 
However,  from  the  constant  brightness  constraint,  only  flow  components  in  the  directions  of  im¬ 
age  gradients,  i.e.  normal  flow,  can  be  computed  directly.  Therefore,  additional  constraints  are 
introduced  and  regularization  techniques  are  applied  for  the  recovery  of  optical  flow  [26,  30,  39]. 
Techniques  employing  spatial-temporal  filters  for  the  recovery  of  full  flow  have  also  been  considered 
[24].  Using  the  second  assumption,  motion  and  structure  are  extracted  from  the  computed  opti¬ 
cal  flow  [1,  4,  35,  49,  54].  Alternatively,  without  the  explicit  computation  of  optical  flow,  direct 
methods  resorting  to  the  direct  estimation  of  motion/structure  [3,  25,  40,  47]  and  the  qualitative 
interpretation  of  visual  motion  [21, 42]  have  also  been  pursued.  Although  great  advances  have  been 
made  in  this  area,  the  instability  and  sensitivity  to  noise  in  estimating  image  derivatives  impose 
limits  on  flow-based  methods. 

Feature-based  approaches,  on  the  other  hand,  rely  on  the  correspondence  of  sparse  features  to 
recover  motion/structure.  Depending  on  the  application,  different  features  such  as  points,  lines  and 
contours  can  be  used.  Since  less  stringent  constraints  than  the  direct  use  of  the  constant  brightness 
constraint  are  usually  employed  to  obtain  the  feature  displacements,  more  robust  interpretation  of 
motion/structure  becomes  possible.  In  addition,  much  of  the  early  work,  the  so-called  two-frame 
based  approaches,  focused  on  using  two  or  three  frames  in  their  formulations,  resulting  in  simple 
linear  algorithms  [34,  43,  50-52].  However,  a  lack  of  robustness  of  the  estimates  to  correspondence 
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errors  has  been  observed  [2,  20,  28].  One  way  in  which  robust  algorithms  can  be  obtained  is  by 
exploiting  the  dynamic  nature  of  the  sequence,  i.e.  the  long-sequence  based  schemes. 

Most  of  the  long-sequence  based  approaches  have  employed  kinematic  laws  for  capturing  the 
dynamic  nature  of  a  sequence  [7,  46,  56,  58-60].  The  motion  of  a  camera  is  commonly  assumed  to 
be  smooth  in  these  approaches:  in  [7],  a  smooth  motion  model  such  as  constant  translation  and 
constant  precision  motion  is  assumed;  in  [46],  the  motion  is  assumed  to  be  constant  over  the  relevant 
image  sequence,  in  which  different  types  of  motion  such  as  pure  rotation,  dominant  translation, 
etc.  are  considered;  in  [56],  a  Taylor  series  expansion  is  employed  to  describe  the  trajectory  of  the 
rotation  center;  in  [59],  a  constant  acceleration  and  precision  motion  model  is  assumed;  in  [60]  a 
constant  acceleration  and  rotation  model  is  used,  while  in  [58]  a  constant  translation  and  rotation 
kinematic  law  is  adopted.  For  applications  involving  indoor  surroundings  or  outdoor  environments 
with  flat  surfaces,  motion  may  be  satisfactorily  described  by  a  kinematic  law  [17].  However,  in 
applications  such  as  off- road  vehicular  navigation  and  automatic  flight  landing  [44],  the  excitation 
of  disturbances  results  in  the  deviation  of  the  underlying  motion  from  the  assumed  kinematic 
model.  Consequently,  the  interpretation  of  motion/structure  is  afiected  by  the  presence  of  these 
disturbances  [18]. 

We  are  therefore  interested  in  employing  more  appropriate  dynamic  laws  for  capturing  the 
behavior  of  mobile  robots.  Assume  that  the  translation  of  a  vehicle  is  mainly  along  its  longitudinal 
axis  and  its  speed  can  easily  be  obtained  from  an  odometer,  we  concentrate  on  dynamic  laws 
for  describing  the  rotation  of  a  vehicle.  The  orientation  of  a  vehicle,  in  general,  results  from 
a  desired  smooth  rotation  and  an  undesired  high  frequency  oscillatory  rotation.  The  smooth 
rotation  arises  from  the  steering  command  as  well  as  the  natural  response  of  a  vehicle  when  it 
traverses  a  flat  surface.  The  oscillatory  rotation  is  the  residual  response  of  mechanical  elements 
to  the  roughness  components  in  natural  terrain.  More  specifically,  the  oscillatory  rotation  is  part 
of  the  undesired  motion  which  the  suspension  system  is  designed  to  compensate.  Although  sensors 
such  as  gyroscopes  and  accelerometers  can  be  used  to  compensate  for  these  effects,  this  results  in 
a  costly  system.  When  dynamic  laws  are  available  to  describe  these  phenomena,  the  separation  of 
both  rotational  phenomena  directly  from  the  imagery  becomes  possible.  We  refer  to  this  procedure 
as  selective  stabilization. 

This  dissertation  therefore  deals  with  various  issues  related  to  the  robust  recovery  of  motion 
and  structure  parameters  when  a  vehicle  performs  off-road  navigation,  namely  image  stabilization, 
selective  stabilization,  and  tracking  of  image  primitives. 

1.1.1  Image  Stabilization 

Image  stabilization,  defined  here  as  the  warping  of  video  sequences  for  the  removal  of  image  motion 
due  to  camera  rotation,  is  a  key  preprocessing  step  in  dynamic  image  analysis.  This  procedure 
is  important  for  various  reasons.  The  most  significant  reason  is  that  the  rotational  flow  does  not 
convey  structural  information  and  image  motion  due  to  camera  translation  can  often  be  confused 
with  flow  resulting  from  camera  rotation  [48].  Image  stabilization  is  therefore  useful  for  motion 
analysis,  structure  from  motion  [29,  31],  as  weU  as  the  recovery  of  the  Focus  of  Expansion  (FOE) 
and  other  structural  information  (such  as  time-to-coUision).  After  performing  stabilization,  simple 
and  effective  independent  motion  detection  mechanisms  can  be  employed  to  detect  independently 
moving  objects  [41].  In  addition,  image  stabilization  is  also  important  for  visual  control,  as  well 
as  for  other  image  exploitation  tasks  such  as  registration,  object  detection  [62],  automatic  target 
recognition,  autonomous  vehicle  navigation  [15],  and  model-based  compression  [32]. 

In  this  dissertation,  we  study  the  use  of  combined  visual  cues  and  dynamical  models  for  the 
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purpose  of  stabilization,  and  rely  on  an  Extended  Kalman  Filter  (EKF)  for  the  estimation  of  the 
parameters  of  interest.  Since  the  image  motion  of  features  such  as  distant  points  and  horizon 
Unes  is  mainly  affected  by  the  rotational  motion  of  the  camera,  both  visual  cues  are  exploited  in 
either  calibrated  or  uncalibrated  stabilization  scheme.  For  the  caUbrated  scheme  where  the  intrinsic 
parameters  such  as  the  focal  length  and  the  intersection  of  the  optical  axis  on  the  image  plane  are 
known,  the  imaging  model  is  applied  to  a  3D  vector  whose  motion  characterizes  the  movement  of  a 
horizon  hne.  Consequently,  we  unify  the  representations  of  points  and  hnes  for  the  estimation.  For 
sequences  whose  intrinsic  parameters  are  unknown,  the  uncalibrated  scheme  alternatively  exploits 
the  image  motion  of  measurable,  2D  normal  vectors  of  horizon  lines.  Moreover,  the  study  of  the 
sensitivity  of  relevant  parameters  with  respect  to  the  intrinsic  parameters  yields  similar  approaches 
in  both  calibrated  and  uncahbrated  schemes.  The  issues  of  local  and  global  stabilization  schemes 
are  addressed.  The  residual  motion  remaining  in  a  stabilized  sequence  is  also  analyzed.  Finally, 
both  stabilization  schemes  have  been  applied  to  several  real  sequences,  and  the  results  have  been 
made  available  to  interested  readers. 

1.1.2  Selective  Stabilization 

As  discussed  earher,  traditional  approaches  to  the  recovery  of  motion  and  structure  parameters 
have  regarded  the  effects  of  high  frequency  oscillatory  motion  as  noise.  As  a  result,  the  estimates 
obtained  without  the  exphcit  consideration  of  these  disturbances  are  less  desirable  for  higher  level 
applications  such  as  control  and  planning. 

In  this  dissertation,  we  consider  both  kinematic  and  kinetic  models  suitable  for  capturing  dif¬ 
ferent  phenomena  and  achieving  the  separation  of  smooth  rotation  and  oscillatory  rotation.  Our 
approach  uses  various  dynamic  laws  to  model  the  behavior  of  a  vehicle,  and  relies  on  an  EKF  for  the 
estimation  of  both  phenomena.  In  addition  to  the  apphcation  of  the  proposed  image  stabilization 
technique,  a  scheme  for  detecting  the  beginning  and  end  of  smooth  rotation  is  devised.  Appropriate 
dynamic  laws  are  then  employed  to  achieve  selective  stabilization.  Based  on  selective  stabiUzation, 
the  3D  locations  of  close  feature  points  are  estimated  in  a  stabilized  frame  of  reference,  thus  pro¬ 
viding  more  useful  information.  Synthetic  experiments  for  dilferent  scenarios  using  the  proposed 
approach  show  promising  results. 

1.1.3  Feature  Tracking 

Feature  tracking  over  a  sequence  is  the  first  step  in  feature-based  approaches  for  the  recovery  of 
motion  and  structure  parameters.  Many  feature  tracking  algorithms  assume  two  steps  in  tracking 
discrete  features.  The  first  step  is  to  detect  features  using  a  feature  detection  scheme.  Subsequently, 
different  algorithms  employ  different  methods  in  matching  a  set  of  currently  tracked  features  to  the 
set  of  features  detected  in  the  previous  step.  The  accuracy  of  these  algorithms  thereupon  relies  on 
the  feature  detection  schemes. 

Since  detecting  the  same  features  in  subsequent  frames  is  not  easy,  we  propose  a  localized  feature 
tracking  algorithm.  The  scheme  identifies  corresponding  points  using  other  techniques  to  avoid  the 
above  problem.  The  trajectory  of  each  feature  point  is  first  described  by  a  2D  kinematic  model. 
Then,  to  track  a  feature  point,  an  inter-frame  motion  estimation  scheme  is  designed  to  obtain 
estimates  of  inter-frame  motion  parameters.  Using  these  estimates,  corresponding  points  are  then 
identified  to  sub-pixel  accuracy.  Temporal  information  is  processed  to  facilitate  subsequent  tracking. 
Since  different  feature  points  are  tracked  independently,  the  algorithm  is  able  to  handle  image 
motion  arising  from  general  3D  camera  movements.  In  addition  to  tracking  feature  points  detected 
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at  the  beginning,  an  efficient  way  to  dynamically  include  new  points  extracted  in  subsequent  frames 
is  devised  so  that  the  information  in  a  sequence  is  preserved.  The  experimental  results  on  many  real 
sequences,  including  the  ones  used  for  achieving  image  stabilization,  have  shown  good  performance 
of  the  algorithm. 

1.2  Contributions 

The  main  contributions  of  this  dissertation  are  the  careful  considerations  of  various  issues  critical 
to  off-road  vehicular  navigation.  Specific  contributions  include  the  following: 

•  A  generic  approach  exploiting  multiple  visual  cues  for  image  stabilization  is  proposed. 

-  The  observability  of  warping  parameters  from  different  image  primitives  (points  and 
lines  at  infinity)  for  both  calibrated  and  uncalibrated  cases  is  studied. 

-  The  sensitivity  of  the  warping  parameters  with  respect  to  the  intrinsic  parameters  for 
uncalibrated  cases  is  investigated. 

-  The  equivalence  of  both  local  and  global  stabilization  schemes  is  discussed.  The  nature 
of  the  residual  motion  in  the  resulting  warped  sequence  is  analyzed. 

-  Different  visual  cues  are  integrated  for  the  estimation  of  rotational  motion  in  a  sequence. 

•  A  scheme  which  explicitly  incorporates  the  oscillatory  behavior  of  a  vehicle  when  it  undergoes 
off-road  navigation  is  designed. 

-  A  kinetic  law  for  oscillatory  rotation  is  derived.  Its  performance  is  studied  both  analyt¬ 
ically  and  quantitatively. 

-  Separability  of  smooth  rotation  and  oscillatory  rotation  is  investigated.  Selective  removal 
of  oscUlatory  rotation  from  total  rotation  is  justified. 

-  Visual  cues  are  exploited  to  instantaneously  or  conservatively  detect  the  beginning  and 
end  of  smooth  rotation. 

-  The  relative  positions  of  scene  points  are  estimated  in  a  stabilized  frame  of  reference. 
Impressions  of  steering  and  climbing  are  preserved  in  the  interpretation  of  visual  motion. 

•  Finally,  a  localized  tracking  scheme  which  performs  tracking  of  a  dynamic  set  of  feature  points 
over  a  sequence  is  proposed. 

-  A  localized  2D  motion  model  which  can  handle  the  image  motion  due  to  general  3D 
movements  of  a  camera  is  presented. 

-  A  scheme  for  tracking  new  feature  points  detected  from  subsequent  frames  is  designed. 
The  scheme  is  efficient  in  the  sense  that  only  limited  numbers  of  feature  points  are 
tracked  at  all  times. 

1.3  Organization 

The  organization  of  this  dissertation  is  as  follows.  Chapter  2  reviews  related  work  on  image  stabi¬ 
lization  and  feature  tracking.  Chapter  3  describes  the  proposed  image  stabilization  scheme.  The 
recovery  of  motion/structure  from  selective  stabilization  is  presented  in  Chapter  4.  Chapter  5  de¬ 
scribes  the  localized  feature  point  tracking  algorithm.  Chapter  6  concludes  this  dissertation  with 
discussions  of  possible  future  directions. 
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Chapter  2 


Related  Work 


In  tills  chapter,  we  review  related  work  on  image  stabilization  and  feature  tracking. 


2.1  Image  Stabilization 

As  mentioned  earlier,  image  stabilization  is  referred  as  the  process  of  the  removal  of  unwanted 
image  motion.  The  unwanted  image  motion  can  be  due  to  the  total  camera  motion  or  only  the 
rotational  motion. 

When  the  removal  of  total  image  flow  is  concerned,  image  stabilization  has  a  similar  meaning 
to  image  registration.  Many  techniques  for  achieving  image  registration  can  therefore  be  applied 
[8].  Among  them,  techniques  pursued  in  [10,  23]  have  attracted  a  lot  of  attention.  Instead  of 
registering  the  current  image  to  the  previous  image,  image  stabilization  is  achieved  by  registering 
the  new  frame  to  a  mosaic  constructed  from  previous  frames.  Specifically,  the  motion  between  the 
current  frame  and  the  mosaic  is  estimated  by  a  multi-resolution,  iterative  process.  During  each 
iteration,  cross-correlation  based  techniques  are  employed  to  compute  the  optical  flow  between  the 
source  image  and  the  warped  image.  Since  the  mosaic  is  used,  the  overlap  between  two  consecutive 
images  in  the  source  video  can  therefore  be  small. 

On  the  other  hand,  image  stabilization  is  sometimes  referred  to  as  the  removal  of  image  flow 
due  to  rotation.  Recently,  in  addition  to  approaches  to  the  estimation  of  3D  motion  of  a  camera, 
a  few  promising  approaches  to  this  type  of  image  stabilization  have  been  proposed  [15,  29,  38,  53]. 

In  [29],  assuming  that  the  image  region  of  a  planar  patch  in  a  3D  scene  is  detected,  the  algorithm 
first  computes  a  quasi-projective  transformation  which  describes  the  image  motion  of  such  a  region. 
Then,  by  applying  the  computed  transformation  to  the  whole  image,  the  rotation  between  the 
current  frame  and  the  previous  frame  is  removed.  Subsequently,  in  order  to  obtain  a  stabihzed 
sequence  as  if  taken  from  a  camera  undergoing  translation,  the  translation  of  the  camera  needs  to 
be  estimated.  The  angular  velocity  of  the  camera  is  computed  afterwards,  and  applied  to  warp  the 
images  to  achieve  image  stabilization. 

In  [15],  two  methods  using  normal  flow  for  the  recovery  of  3D  motion  parameters  are  proposed. 
The  first  approach  uses  the  pattern  of  normal  flow  to  estimate  the  motion  of  the  camera.  It  is 
regarded  as  a  scheme  for  estimating  the  ego-motion  of  a  camera.  The  other  approach  focuses  on 
the  estimation  of  the  angular  velocity.  Since  the  image  motion  corresponding  to  scenes  which  are 
far  away  is  mainly  due  to  the  camera  rotation,  this  scheme  relies  on  such  image  regions  for  the 
estimation.  In  order  to  obtain  high  accuracy,  iterations  are  performed  to  refine  estimates  of  the 
angular  velocity  using  the  source  frame  and  the  warped  image. 
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In  [53],  with  the  help  of  inertial  sensors,  image  stabilization  is  achieved  by  first  aligning  line 
segments  extracted  from  an  image  sequence  with  the  absolute  vertical  direction.  Disparities  between 
two  successive  frames  are  then  compensated  for  by  2D  linear  translations. 

Finally,  the  removal  of  rotation  can  also  be  achieved  using  image  registration.  In  [38],  instead 
of  using  quasi-projective  transformation  to  describe  the  image  motion,  an  affine  transformation  is 
employed  to  model  the  image  flow  of  distant  features.  Since  the  Euclidean  distances  of  features  are 
affected  only  by  the  scale  between  two  frames,  the  scaling  factor  is  first  estimated.  The  remaining  2D 
translational  and  rotational  parameters  are  then  computed  by  solving  an  over-determined  system. 
Since  the  scheme  compensates  for  the  image  motion  of  distant  features,  it  effectively  cancels  out 
the  rotation  of  the  camera. 


2.2  Feature  Tracking 

Existing  techniques  for  tracking  a  set  of  features  over  a  sequence  of  images  generally  faU  into  two 
categories:  two-view  based  and  multiple-view  based.  Similar  to  the  recovery  of  3D  motion,  when 
the  motion  of  a  camera  is  smooth  such  that  the  smoothness  constraints  hold,  multiple- view  based 
approaches  are  likely  to  outperform  two- view  based  methods.  On  the  contrary,  if  the  movement  of 
a  camera  varies  often  and  result  in  non-smooth  image  motion,  two-frame  based  schemes  seem  to 
capture  the  variations  more  promptly. 

For  two-view  based  approaches,  finding  feature  correspondences  over  a  sequence  of  images  is 
broken  into  successive,  yet  independent  problems  of  two- view  matching  [14, 55, 61].  In  [55],  multiple 
attributes  of  each  image  point  such  as  intensity,  edgeness  and  cornerness  which  are  invariant  under 
rigid  motion  in  the  image  plane  are  used  along  with  a  set  of  constraints  to  compute  a  dense 
displacement  field  and  occlusion  areas  in  two  images.  An  intensity-based  cross-correlation  method 
is  then  used  to  refine  the  two- view  matching  results  and  obtain  feature  point  correspondences  over 
the  sequence  [14].  In  [61],  an  image  registration  technique,  based  on  an  affine  transformation,  is 
first  applied  to  compensate  for  the  motion  of  the  camera  between  two  consecutive  frames.  The 
feature  point  correspondence  problem  is  then  solved  by  repeatedly  identifying  the  corresponding 
points  to  sub-pixel  accuracy  using  a  correlation  matching  method. 

Multiple-view  based  approaches  employ  smoothness  constraints  to  exploit  the  temporal  infor¬ 
mation  existing  in  the  sequence  [6, 11,  45,  60].  In  [45],  under  the  assumption  that  the  motion  of  an 
object  does  not  change  abruptly,  the  correspondence  problem  was  formulated  as  an  optimization 
problem.  The  trajectories  of  a  set  of  feature  points  were  obtained  by  searching  for  a  set  of  trajec¬ 
tories  each  of  which  had  maximal  smoothness.  In  [6],  Multistage  Hypothesis  Testing  (MHT)  was 
applied  to  detect  small,  moving  objects  in  each  image;  a  feature  trajectory  was  determined  by  re¬ 
peatedly  detecting  the  same  feature  point  over  the  sequence.  In  [11],  a  2D  kinematic  motion  model 
was  assumed,  while  the  Joint  Probabilistic  Data  Association  Filter  (JPDAF)  was  later  employed  to 
track  line  segments  with  the  ability  to  initiate  or  terminate  the  trajectory  of  a  line  segment.  In  [60], 
assuming  3D  kinematic  motion  model  and  a  Mahalanobis  distance  based  matching  criterion,  an 
EKF  was  used  to  track  a  set  of  line  segments.  A  fading  memory  type  statistical  test  was  suggested 
to  take  into  account  the  occlusion  and  disappearance  of  line  segments. 
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Chapter  3 


3D  Model-Based  Image  Stabilization 


3.1  Introduction 

Image  stabilization  is  a  key  preprocessing  step  in  dynamic  image  analysis  and  deals  with  the  removal 
of  unwanted  image  motion  in  a  video  sequence.  Depending  on  the  final  application,  this  unwanted 
motion  may  correspond  to  part  or  all  of  the  image  flow  generated  by  the  3D  camera  motion.  If  the 
ultimate  goal  is  feedback  control  of  the  mechanical  system,  the  undesired  image  motion  corresponds 
to  the  residual  oscillatory  motion  of  the  platform  hosting  the  camera.  Image  stabilization  then  has 
a  purpose  similar  to  that  of  mechanical  stabilization.  If  the  task  consists  of  detecting  independently 
moving  objects,  the  unwanted  image  motion  is  instead  that  motion  resulting  from  camera  rotation 
[15] .  If  the  goal  is  mosaicking  and  change  detection  [10, 23],  the  unwanted  image  motion  corresponds 
to  the  total  image  flow.  In  this  chapter,  stabilization  is  principally  understood  as  the  warping  of 
video  sequences  for  the  removal  of  image  motion  due  to  total  camera  rotation.  The  use  of  combined 
visual  cues  and  dynamical  models  for  the  stabilization  of  calibrated  or  uncalibrated  image  sequences 
are  described. 

Parameters  relevant  for  image  warping  are  estimated  by  combining  information  from  different 
tracked  tokens,  namely  points  and  horizon  lines.  These  parameters  are  simply  the  camera  rota¬ 
tional  velocity  if  intrinsic  camera  parameters  are  available,  or  the  projectivity  coefficients,  in  the 
uncalibrated  case.  Image  plane  displacements  of  distant  feature  points  may  unambiguously  char¬ 
acterize  rotational  motion.  However,  such  points  are  sometimes  difficult  to  detect  and  track,  due 
to  the  absence  of  sufficient  intensity  gradient  information.  For  the  same  reason  flow-based  methods 
suffer  from  a  lack  of  available  visual  information.  Horizon  lines,  when  present,  constitute  on  the 
other  hand  very  strong  visual  cues,  requiring  relatively  simple  operations  for  their  tracking.  These 
tokens  must  however  be  combined  so  as  to  remove  all  ambiguities  concerning  camera  motion.  These 
issues  are  addressed  in  the  next  section. 

Subsequently,  we  address  the  important  issue  of  the  selection  of  an  appropriate  dynamic  model 
suitable  for  exploiting  the  temporal  information  in  a  sequence.  In  actual  appUcations,  cameras  are 
often  rigidly  mounted  on  platforms.  The  rotation  of  the  camera  therefore  arises  from  the  rotational 
movement  of  the  host  at  all  times.  We  evaluate  analytically  the  use  of  kinetic  versus  kinematic 
laws  for  the  estimation  of  rotational  motion  components.  We  discuss  the  conditions  under  which 
the  use  of  simpler  kinematic  laws  yields  satisfactory  performance. 

These  analytical  results  are  applied  to  the  stabilization  of  images  acquired  from  off-road  vehicles, 
for  which  the  rotational  motion  is  significant.  Specifically,  to  stabilize  a  sequence,  horizon  lines  are 
first  extracted  from  each  frame  while  distant  feature  points  close  to  the  horizon  are  detected. 
Both  image  primitives  are  tracked  over  the  sequence.  The  matched  lines  and  points  thereafter 
form  a  set  of  visual  cues.  Observations  are  then  used  along  with  a  kinematic  law  to  estimate  the 
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needed  warping  parameters.  Based  on  the  estimated  warping  parameters,  a  stabilized  sequence  is 
generated. 

The  organization  of  this  chapter  is  as  follows.  Section  3.2  discusses  the  observability  of  warping 
parameters  from  different  visual  cues.  Residual  motion  of  warped  sequences  is  analyzed  in  Sec¬ 
tion  3.3.  Section  3.4  compares  different  dynamics-based  estimators  for  the  estimation  of  rotation. 
An  integrated  approach  exploiting  temporal  information  as  well  as  multiple  visual  cues  is  presented 
in  Section  3.5.  Section  3.6  reports  experimental  results  on  real  image  sequences.  Conclusions  are 
given  in  Section  3.7. 

3.2  Model-guided  Image  Warping  Schemes 

This  section  addresses  the  observability  of  parameters  used  for  image  stabilization  in  both  calibrated 
and  uncaJibrated  cases.  We  address  the  recovery  of  parameters  from  points  and  horizon  Unes. 
Consider  the  scenario  shown  in  Figure  3.1  where  a  camera  undergoes  rotation  with  instanUneous 
angular  velocity  w  ;  translation  with  linear  velocity  V  .  {Vx,Vy,Vz)  •  Let 

P  :  (X,y,  Z)'^  denote  the  3D  position  of  a  scene  point  with  respect  to  the  camera,  and  p  :  {x,y)  , 
the  image  plane  coordinates  of  the  corresponding  projection  point. 


X 


Figure  3.1:  The  camera  motion  and  imaging  model. 

The  relative  motion  of  the  scene  point  with  respect  to  the  camera  is  then  described  by 

P  =  -c.;xP-V  (3-1) 

Assuming  that  the  perspective  projection  is  used  as  an  imaging  model,  p  is  related  to  P  as  follows: 

p  =  p(P)-HPc  (3-2) 

where  Pc  is  the  intersection  of  the  optical  axis  with  the  image  plane  and  V  denotes  the  perspective 
projection  operator,  i.e.  ^  ^  ^ 

(3.3) 


P(P) 


f  ^ 
J<^z  J 
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with  fc  the  focal  length.  Consequently,  the  image  motion  arising  from  the  camera  movement 
satisfies  (see  Appendix  A  for  derivations) 


P  =  [fc  -  PcVxy  (p  -  Pc)  +  f^zip  -  Pc)  +  fc^xy]  +  [(p  -  Pfoe)-] 


Pr 


Pt 


(3.4) 


where  pfoe  =  ^(V)  +  and  r  =  Z/Vg  respectively  correspond  to  the  Focus  of  Expansion  (FOE) 
and  the  time-to-collision  to  the  imaged  point;  fig  is  the  2x2  skew- symmetric  matrix  related  to 
the  rotational  velocity  component  along  the  optical  axis 


i2g  — 


0  Ug  _ 

-cog  0  ’ 


(3.5) 


and  is  an  image  vector  orthogonal  to  the  projection  of  the  instantaneous  angular  velocity 
component  parallel  to  the  image  plane, 


U} 


j. 

xy 


(3.6) 


When  image  motion  is  described  instantaneously,  image  velocities  p  due  to  3D  rotation  of  the 
camera  are  expressed  as  second  order  polynomial  functions  of  image  positions  and  are  independent 
of  depth.  For  this  reason,  it  is  a  well  known  fact  that  image  motion  resulting  from  rotation  can  be 
instantaneously  compensated  for.  Care  has  to  be  taken,  however,  with  regard  to  the  interpretation 
of  the  resulting  derotated  sequence,  a  point  addressed  in  the  next  section.  On  the  other  hand, 
unless  relative  depth  is  known,  or  aU  imaged  points  lie  on  the  same  3D  plane,  translational  motion 
cannot  be  compensated  for. 

Consider  a  distant  point  (i.e.  let  r  oo)  and  denote  its  position  by  P.  As  seen  from  (3.4)  such 
points  move  relative  to  the  camera  as  if  only  rotation  were  present.  Therefore  we  may  equivalently 
assume  for  these  points  that  the  3D  motion  simply  satisfies 


P  =  -oj  X  P  (3.7) 

For  off-road  vehicle  navigation,  or  images  taken  from  a  plane  or  a  helicopter,  horizon  lines  or  partial 
profiles  of  objects  lying  far  away  constitute  very  strong  visual  cues.  In  Figure  3.2,  consider  an  image 
horizon  hne  denoted  by  £;  £  is  uniquely  characterized  by  W,  the  3D  vector  normal  to  the  plane  11 
through  £  and  the  camera  center. 

Since  the  image  motion  of  horizon  lines  is  explained  exclusively  in  terms  of  the  camera  rotation, 
it  follows  that  the  motion  of  the  normal  vector  W  is  itself  described  by 

W  =  -a;  X  W  (3.8) 

If  u  is  one  solution,  i.e.  W  =  — u  x  W,  then  kW  -f-  u  also  satisfies  (3.8)  for  any  k.  Therefore, 
observing  the  image  motion  of  one  horizon  hne  characterizes  the  rotational  component  on  plane 
n  only.  There  is  indeterminacy  along  the  direction  W.  Given  one  observation,  the  set  of  possible 
solutions  of  (3.8)  describes  an  affine  hne  in  3D  rotational  parameter  space.  When  only  one  horizon 
hne  is  observed,  rotation  may  be  determined  in  the  Least  Square  (LS)  sense  which  corresponds  to 
the  camera  motion  with  least  energy  that  explains  the  image  motion  of  that  particular  hne.  In  this 
case  rotational  motion  inducing  image  motion  along  the  hne  feature  itself  (lateral  motion)  is  not 
always  totally  compensated  for  (since  it  is  assumed  to  be  zero),  and  other  hnes  or  geometrical  cues 
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Figure  3.2:  Geometric  representation  of  a  horizon  line  £,  the  plane  H,  the  3D  normal  vector  W 
and  the  image  plane  normal  vector  o. 

near  the  horizon  can  be  used  to  qualify  lateral  motion  for  full  stabilization.  Indeterminacy  exists 
also  if  only  one  distant  point  is  observed.  In  this  case,  this  indeterminacy  involves  the  rotational 
component  along  a  ray  from  the  image  center  to  this  point.  Qualitatively,  however,  lines  and  points 
carry  equal  amounts  of  information  in  the  determination  of  rotation.  If  any  combination  of  two  of 
these  features  is  observed,  rotation  can  be  fuUy  characterized,  except  in  some  degenerate  cases  for 
which  W  X  P  =  0,  which  in  practice  cannot  occur  unless  the  observer  possesses  an  unreasonably 
large  Field  of  View  (FOV).  Letting  w  =  P(W)  +  pc,  one  may  solve  the  over-determined  linear 
system 

Qu>  =  D  (3.9) 

where  D  =  [pf, . .  .,p^wf , . . .,  w^]^,  while  Q  is  a  matrix  derived  from  (3.4): 

(xi  -  a;c)(t/i  -  Vc)  -(/c  +  (a:i  -  «c)^)  ivi  -  Vc) 

{fc  +  fc  ~  VcY)  ~fc  ^(^1  —  ^c)(yi  ~  Vc)  ~(2:i  —  Xc) 

(XM  -  Xc){yM  -  Vc)  -{fc  +  {XM  -  Xcf)  iVM  ”  Vc) 

Q=  {fc  + fc^iVM  -  Vcf)  -fc^{xM  -  Xc){yM  -  Vc)  -{xM  -  Xc)  (3.10) 

(w^i  -  a:c)(Wj^i  -  Vo)  -{fc  +  (W:ri  -  aJc)^)  (Wj,j  -  Vc) 

{fc  +  fr^{^yi  —  yc)^)  —fc  ^('*''^271  ~  2;c)(Wyj  —  yc)  —{'^Xl  —  Xc) 

.  {fc  +  fcK'^VN  -  ycf)  -fc^{^^N  -  ^c){^yN  -  Vc)  -{'^xr,  -  Xc)  . 

Line  features  can  therefore  be  combined  with  other  tracked  tokens  such  ais  distant  points  for  stabi¬ 
lization. 

We  now  discuss  the  image  warping  schemes.  Irrespective  of  the  particular  trajectories  of  the 
vectors  a>(t)  and  V(t),  the  positions  of  3D  points  at  two  time  instants  can  always  be  described  by 
an  element  of  the  Special  Euclidean  group  SE{Z)  (uniquely,  if  the  rotation  center  is  given),  i.e., 
there  exist  a  total  rotation  R  :  =  l,...,3,y  =  1, ...,3  and  translation  T  :  {Tx,Ty,Tz)'^, 

between  any  two  frames,  such  that  the  3D  point  positions  P i  and  P 2  expressed  in  the  camera 
frame  of  reference  satisfy 

P2  =  RPi  +  T  (3.11) 
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As  before,  for  a  distant  point,  the  contribution  frona  the  translation  T  is  negligible.  The  image 
plane  positions  of  such  a  point  at  ti  and  t2  are  then  expressed  as 


P2  =  (c^Pi  +  1)  ^Api  +  b) 


where 


A 

b 

c 

d 


d-^ 


fcTll  +  Xcrsi 

fcr21  +  VcTzi 


/c7’12  +  Xcrz2 

ail  ai2 

fcr22  +  ycrz2 

«21  0,22 

d 


1 

-fcXcTll  -  fcVcTu  +  fcXiz  -  xlrsi  -  Xcyc'rz2  +  fcXcTzs, 

'  bi  ■ 

-fcXcr21  -  fcycr22  +  /c  ^23  “  “  2/c^32  +  fcVcTzS 

62 

d 


1 

^31 

Cl 

rz2 

C2 

-XcTsi  -  ycrz2  +  fcTzZ 


(3.12) 

(3.13) 

(3.14) 

(3.15) 

(3.16) 


Similarly,  for  a  distant  line  feature  characterized  by  the  projected  normal  vector  w, 

W2  =  (c^wi  +  l)“^(Awi  +  b) 


(3.17)  ' 


As  seen  from  (3.12)  and  (3.17),  for  distant  features,  their  image  plane  motions  are  described  by 
projective  group  operations.  In  fact,  this  is  expected  since  points  on  the  horizon  fall  into  a  plane  at 
infinity  and  the  image  motion  of  planes  is  described  exactly  by  a  projectivity  [1].  Consequently,  if 
{rij,  i  =  1, . . . ,  3,  j  =  1, . . . ,  3}  in  A,  b  and  c  are  estimated  from  points  and  lines  near  the  horizon, 
we  can  compensate  for  the  rotation  between  two  images  using  the  transformation 

Pc2  =  (P2C^  -  A)"^(b  -  P2)  (3.18) 

=  (c^P2  +  l)“^(Ap2  +  b)  (3.19) 


where  Pc2  represents  the  points  on  the  compensated  image.  This  is  also  a  projective  transformation 
with  parameters  A,  b  and  c,  a.s  expected  by  virtue  of  the  projective  group  property.  It  is  shown 
later  that  there  exists  only  a  translation  between  stabilized  images.  Therefore,  (3.19)  forms  the 
basis  of  our  stabilization  scheme  for  calibrated  sequences. 

Note  that  simpler  image  transformations  have  been  used  in  the  literature  for  stabilization  pur¬ 
poses:  the  SE(2)  group  of  transformation  and  aIRne  transformations,  i.e.  Pc2  =  Ap2  -f  b,  are  some 
examples  used  in  works  such  as  [15, 29];^  these  are  essentially  appropriate  in  cases  of  parallel-frontal 
motion. 

In  cases  where  the  intrinsic  parameters  are  unknown,  we  show  that  it  is  stiU  possible  to  achieve 
image  stabilization  using  both  distant  points  and  fines.  For  points,  since  p  is  directly  measurable 
from  the  images,  the  projective  transformation  in  (3.12)  remains  unchanged.  However,  due  to  the 
unknown  pc  and  /c,  the  mapping  for  distant  fines  in  (3.17)  is  no  longer  applicable  since  w  is  no 
longer  measurable.  Consider  instead  the  measured  image  normal  vector  to  a  line  near  the  horizon 
described  in  the  image  plane  by 

o^p  =  1  (3.20) 

It  is  shown  in  Appendix  B  that  o  is  related  to  the  previously  defined  projected  normal  w  by 

w  = -(1- <  pc,o  >)~^/cO-kpc  (3.21) 

^In  [29],  a  second  order  polynomial  quasi- projective  transformation  is  assumed,  pet  =  Cpp^d-f- Ap  +  b;  an  affine 
transformation  is  then  used  for  further  motion  analysis. 
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Using  the  identities  o|’p2  =  1  and  ofpi  =  1  along  with  (3.12)  we  can  further  show  that  the 
movement  of  o  also  satisfies  a  projective  transformation  whose  parameters  are  related  to  the  inverse 
projective  transformation  A,  b,  c  in  (3.19): 

02  =  (-b^oi  +  l)“^(A^oi  -  c)  (3.22) 


Consequently,  image  stabilization  of  uncalibrated  sequences  can  be  carried  out  by  estimating  the 
eight  parameters  in  A,  b  and  c  from  distant  points  and  lines.  While  this  is  possible  in  principle, 
in  practice,  the  computation  of  the  projectivity  parameters  from  distant  features  can  be  unstable. 
To  see  this,  consider  the  system  obtained  from  (3.12), 


Gf  =  q 


(3.23) 


where  f  is  the  vector  including  the  eight  projectivity  parameters,  f  =  (an, . .  .,C2)^,  q  consists  of 
the  coordinates  of  a  set  of  points  such  as  p2,  and  G  is  a  matrix  with  elements  composed  of  the 
coordinates  of  pi  and  p2.  This  matrix  is  often  ill  conditioned;  it  can  be  shown  that  the  last  two 
columns  of  G  contain  second  order  terms  in  the  image  coordinates,  while  the  third  and  sixth  columns 
contain  zeroth  order  terms.  Geometrically,  since  the  eight  parameters  are  computed  from  features 
lying  only  on  a  constrained  region  of  the  image  (namely  close  to  the  horizon),  there  exist  many 
projectivities  leaving  the  horizon  invariant,  some  of  which  are  not  suitable.  One  possible  solution 
to  this  problem  is  to  further  constrain  the  intrinsic  parameters  by  assuming  an  approximate  value 
and  concentrating  on  estimation  of  the  rotation. 

Indeed,  there  often  are  situations  where  the  intrinsic  parameters  are  approximately  known.^  In 
these  cases,  denote  the  intrinsic  parameters  by  A=  {fc^cVcY ■,  and  let  the  nominal  values  be  Aq. 
If  we  further  assume  that  the  eight  projective  coeflBcients  vary  smoothly  with  respect  to  A  then 


f(A) 


f(Ao)  + 


dx 


(A-Ao) 


f(Ao)  +  J(Ao)(A-Ao) 


(3.24) 

(3.25) 


where  J  is  the  Jacobian  matrix.  When  the  elements  of  J  are  small,  the  effect  of  the  imperfect 
knowledge  of  the  intrinsic  parameters  is  negligible  for  stabilization  purposes.  A  small  error  in  the 
assumed  intrinsic  parameters  will  stiU  lead  to  acceptable  stabilization  results.  For  example,  consider 
the  sensitivity  of  6i  with  respect  to  fc-  Then  from  (3.14)  and  assume  the  optical  axis  intersects  the 
image  plane  at  the  center,  i.e.  (xcj/c)  =  (0?0),  we  have 


dh  _ 

dfc 


-OJv 


(3.26) 

(3.27) 


where  (3.27)  is  typically  true  since  the  rotation  between  two  consecutive  frames  is  small,  say  on 
the  order  of  10"^.  Consequently,  if  the  error  in  fc  is  within  100  pixels,  the  error  in  h  will  be 
within  one  pixel.  Since  similar  arguments  can  be  applied  to  other  coefficients,  we  can  concentrate 
on  estimating  the  three  rotational  parameters  as  in  the  case  of  calibrated  sequences.  It  will  be  seen 
later  that  this  assumption  holds  in  real  applications. 


^In  fact,  the  errors  in  these  parameters  can  be  moderate,  as  shown  in  the  experiments  later. 
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3.3  Residual  Motion  Analysis 

We  analyze  here  the  nature  of  the  resulting  sequence  when  the  image  warping  described  in  the 
previous  section  is  applied.  We  further  compare  two  types  of  stabilization:  local  stabilization  and 
global  stabilization.  For  convenience,  denote  the  sequence  of  original  images  by  =  {/q,  /i, . . . ,  Iri} 
and  call  the  compensated  sequence  Sc  =  {/o?  -^ci  ?  •  •  •  5  Icn}^  To  obtain  a  translation-only  sequence  5c, 
either  a  local  or  global  stabilization  scheme  can  be  utilized.  The  local  approach  directly  compensates 
for  the  rotation  between  i/t+i  a^nd  Ick  to  generate  Ick+i^  while  the  global  method  computes  the 
rotation  between  Ik^i  and  Ik- 

Consider  first  the  residual  motion  in  a  sequence  generated  by  a  local  stabilization  scheme.  Since 
every  frame  /jt+i  is  directly  stabilized  with  respect  to  the  previous  compensated  image  Ick^  we  have 

PcA:+l  =  '^k-\-l,k^  k+1  (3.28) 

where  R^+i  ^  denotes  the  rotation  between  Ick  and  /jt+i,  while  Pk+i  Pcit+i  respectively  rep¬ 
resent  the  3D  coordinates  of  a  scene  point  relative  to  the  camera  coordinate  system  in  Ik+i  and 
Jcfc+i.  Also,  recall  that  the  motion  of  in  the  original  sequence  is  described  by 

k  T^k+l,k  (3.29) 

with  Ra;+i,A;  and  Tk^i,k  the  rotation  and  translation  between  the  camera  frames  of  reference  in  Ik 
and  /fc+i-  Substituting  (3.29)  into  (3.28)  and  using  (3.28)  to  relate  Pck  and  P^,  the  residual  motion 
between  Ick+i  and  Ick  can  be  expressed  as  follows: 

P  c/:+l  “  P  c/;  “i"  Kl+^,kTk+i,k  (3.30) 

Furthermore,  is  related  to  the  rotation  in  the  original  sequence  by 

RC  _  T>  TJ  C 

k+l,k  —  ^k+l,ki^k,k-l 

—  *' '1^1,0  (3.31) 

Therefore,  as  expected,  the  compensated  sequence  exhibits  a  purely  translational  motion.  If 
is  known,  the  compensated  sequence  can  be  used  for  ego-motion  recovery.  Observe,  however, 
that  while  the  magnitude  of  the  translation  in  the  stabilized  sequence  is  the  same  as  that  of  the 

translation  magnitude  in  the  original  sequence,  the  apparent  translational  heading  (and  therefore 

the  FOE)  now  rotates  with  the  original  rotational  motion. 

This  local  stabilization  scheme,  however,  is  not  practical  for  real  applications.  As  seen  from 
(3.31),  fc  in  fact  accounts  for  the  rotation  between  Jq  and  Ik+i-  For  large  A:,  the  motion 
between  Iq  and  Ik+i  is  likely  to  be  large.  The  overlap  between  Ick  and  Ik+i  may  therefore  be  small. 
The  common  features  are  more  difficult  to  find  and  consequently,  it  is  not  easy  to  compute  ^k+i,k 
reliably.  Global  stabilization,  on  the  other  hand,  focuses  on  the  estimation  of  the  rotation  between 
Ik  and  Ik+i',  the  stabilized  image  is  created  afterwards  with  respect  to  the  reference  frame,  say  Iq, 
according  to 

Pcit+i  =  (Rfc+i,jtRic,i-i  •  •  •P'l.o)  ^Pfc+i  (3.32) 

The  sequence  generated  by  the  global  stabilization  scheme  therefore  exhibits  the  same  residual 
motion  as  the  sequence  obtained  using  the  local  stabilization  approach.  However,  in  contrast  with 
the  local  approach,  the  global  stabilization  scheme  is  more  likely  to  provide  reliable  estimates  of 
the  parameters  of  interest  in  real  applications,  since  the  area  of  overlap  between  Ik  and  Ik+i  is 
greater.  This  scheme  is  therefore  employed  in  our  work. 
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3.4  Exploitation  of  Dynamics  for  Full  Stabilization 

The  next  issue  involves  the  robust  estimation  of  the  warping  parameters  used  in  the  stabilization 
scheme  presented  above.  As  argued  earlier,  the  inherent  nature  of  stabilization  implies  that  tempo¬ 
ral  information  present  in  the  sequence  should  be  exploited  for  image  warping  purposes.  A  relevant 
problem  then  becomes  that  of  selecting  suitable  parameter  dynamics  so  as  to  exploit  this  temporal 
information.  We  concentrate  on  the  dynamics  for  the  rotation  parameters.  The  incorporation  of 
the  proposed  dynamic  laws  into  the  estimation  process  is  presented  later  in  Section  3.5. 

We  consider  here  dynamic  laws  appropriate  for  capturing  the  evolution  of  rotational  motion  of 
the  platform  on  which  the  camera  is  mounted.  Since  the  movement  of  the  platform  is  affected  by 
the  interaction  between  mechanical  elements  and  its  environment,  the  motion  is  best  described  by  a 
kinetic  model  which  emulates  the  behavior  of  the  mechanical  system.  The  use  of  simpler  kinematic 
models  is  compared  to  that  of  more  complete  kinetic  models. 

This  section  therefore  starts  with  the  description  of  a  kinetic  model  of  a  vehicle.  The  presenta¬ 
tion  of  this  model  leads  to  the  study  of  selective  stabilization,  which  will  be  addressed  in  the  next 
chapter.  Subsequently,  using  this  model,  the  appropriateness  of  kinematic  laws  for  describing  the 
evolution  of  rotational  parameters  is  evaluated. 

3.4.1  Kinetic  Models 

As  discussed  earher,  the  movement  of  a  vehicle  over  rough  terrain,  in  general,  can  be  decomposed 
into  two  components:  the  smooth  motion  and  the  residual  oscillatory  motion.  The  smooth  motion 
corresponds  to  the  behavior  of  the  vehicle  as  if  the  terrain  were  smooth;  it  includes  translation,  as 
well  as  rotation  due  to  steering  and  climbing.  The  residual  oscillatory  motion  refers  to  the  residual 
vehicular  motion;  it  characterizes  the  response  of  the  vehicle  to  the  roughness  of  the  terrain. 

We  proceed  by  employing  a  four-wheel  vehicle  model  to  account  for  the  residual  oscillatory 
vehicular  movement.  This  model  takes  into  account  the  phenomena  of  bounce,  pitch  and  roll 
(illustrated  in  Figure  3.3),  and  has  been  widely  used  for  the  design  and  analysis  of  suspension 
systems.  AH  tires  are  modeled  by  linear  springs  with  the  same  stiffness  coefficient  Kt-  M^f  and 
Mwr  represent  the  masses  of  unsprung  elements  such  as  the  front  and  rear  wheels  and  their  axles. 
Kf,Cf,Kr  and  Cr  are  the  characteristics  of  the  linear  springs  and  shock  absorbers  modeling  the 
suspension  system. 


Figure  3.3:  The  four-wheel  vehicle  model  [22] 

Assume  that  each  tire  contacts  the  terrain  at  a  point  at  all  times,  there  exist  seven  degrees  of 
freedom:  the  movements  of  unsprung  elements  {a;i,X3,a;5,»7},  the  bouncing  displacement  of  the 
center  of  gravity  of  the  sprung  element  Xc,  the  pitch  angle  9  and  the  roll  angle  (j)  (illustrated  in 
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Figure  3.4,  in  which  Wa  and  Wb  constitute  the  wheel  base,  while  Ts,r  and  Tj^/  are  the  suspension 
tracks).  The  high  frequency  yaw  motion  is  usually  small  during  driving;  it  can  therefore  be  ne¬ 
glected.  Note  that  because  of  the  decomposition  of  the  vehicle’s  movement,  these  oscillatory  states 
are  measured  with  respect  to  the  equilibrium  positions  resulting  from  the  smooth  motion.  Their 
behavior  is  described  in  detail  in  Appendix  C.  Consequently,  if  we  define  x„s  to  be  the  vector 
containing  the  residual  rotational  components. 


A 


Figure  3.4:  The  residual  oscillatory  motion  of  the  four-wheel  vehicle  model:  the  bounce  Xc,  the 
pitch  angle  9,  and  the  roll  angle  (f>. 


(3.33) 

then  Xus  satisfies 

^US  “ 

(3.34) 

where  includes  other  dependent  quantities, 

(3.35) 

with  Xi  =  (xcic)^  and  x„,  =  {xi,xi,X3,xz,Xi,X5,xr,i7)'^-  ^us  and  Fa,  respectively  denote 
constant  matrices  whose  entries  are  related  to  system  parameters  such  as  the  mass  of  the  sprung 
element,  the  moment  of  inertia,  the  spring  characteristics,  etc. 


3.4.2  Rotation  Dynamics 

We  turn  now  to  the  evaluation  of  kinetic  models  (as  compared  with  simple  kinematic  models)  to 
capture  the  evolution  of  rotational  motion.  The  simple  models  are  sometimes  casually  used  without 
further  examination.  We  show  analytically  that  kinetic  models  are  superior  to  kinematic  models, 
but  that  kinematic  models  yield  acceptable  performance  if  sufficient  number  of  visual  observations 
are  available. 

As  mentioned  previously,  the  attitude  change  between  two  time  instants  is  due  to  smooth 
rotation  and  the  residual  oscillatory  motion.  Since  the  smooth  rotation  due  to  climbing  and  banking 
lasts  only  for  a  short  period,  we  study  the  performance  of  a  kinematic-law  based  estimator  for  the 
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estimation  of  pitch  and  roll  motion  only.  The  estimator  can  easily  be  generalized  to  take  into 
account  all  the  rotational  components  (including  the  smooth  motion  components). 

For  simplicity,  let  the  measurements  z  available  for  the  estimation  of  are  related  to  Xus  by 
a  linear  transformation  C, 

z  =  Cxus  (3.36) 

Then  assuming  that  the  residual  oscillatory  rotational  motion  component  is  exactly  described  by 
the  dynamics  of  x^^,  we  compare  the  performance  of  estimators  based  on  different  dynamics  when 
additional  measurements  are  available.  The  kinetic-law  based  estimator  yi  captures  the  oscillatory 
motion  using  coefficients  related  to  the  true  dynamics  [12]: 

i^i  =  -^«.yi  +  Ki[z-Cyi]  (3.37) 


where  yi  represents  the  resulting  estimate  of  x^j,  and  Ki  is  the  desired  gain  matrix.  The  kinematic- 
law  based  estimator  y2,  on  the  other  hand,  assumes  no  knowledge  of  the  underlying  mechanical 
system  and  therefore  only  employs  the  smooth  variation  dynamics 

^2  =  ^y2  +  K2[z-Cy2]  (3.38) 


where  y2  and  K2  respectively  have  meanings  similar  to  those  of  yi  and  Ki ,  while 


#01  0 

0  $01 


with  0  the  2x2  zero  matrix,  and 


^01 


0  1 
0  0 


Let  us  define  the  corresponding  estimation  errors  as 


yi  =  x„s  -  yi 

y2  =  Xus  -  72 

It  can  be  shown  that  yi  and  y2  satisfy 

=  i^us  -  KiC)yi  + 

=  (^  -  K2C)y2  -1-  ^US^US  +  A^ush 


(3.39) 


(3.40) 


(3.41) 

(3.42) 


(3.43) 

(3.44) 


where  -  $  is  the  mismatch  between  the  assumed  and  true  dynamics.  If  Ki  is  chosen 

so  that  the  eigenvalues  of  -  KiC  have  negative  real  parts,  then  yi  remains  bounded  as  long 
as  u  is  bounded.  Similarly,  if  A#„sy2  is,  in  addition,  bounded,  we  can  choose  K2  so  that  y2  is 
bounded.  Nonetheless,  y2  wiU  exhibit  a  larger  error  than  yi. 

The  kinetic-law  estimator,  however,  requires  knowledge  of  the  mechanical  system  parameters. 
These  parameters  are  not  always  easily  measurable.  When  the  system  parameters  are  unknown, 
the  availability  of  sufficient  number  of  visual  measurements  should  allow  for  the  use  of  the  simpler 
kinematic  law  while  still  yielding  good  warping  parameter  estimates.  The  next  section  employs  the 
kinematic  laws  for  the  estimation  of  total  rotation. 
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3.5  Parameter  Estimation 


Various  types  of  estimators  can  be  employed  to  obtain  the  parameters  used  in  our  stabilization 
scheme.  Recursive-type  estimators  update  the  estimates  of  the  parameters  whenever  new  infor¬ 
mation  becomes  available  while  batch-type  estimators  compute  the  estimates  by  processing  all  the 
information.  Because  of  their  ability  to  process  data  sequentially  and  their  lower  computational 
complexity,  recursive-type  estimators  are  preferred  over  batch  estimators  for  real  time  processing. 
Kalman  filters  make  use  of  dynamics  for  their  estimation.  We  therefore  provide  Extended  Kalman 
Filter  (EKF)  formulations  for  both  the  calibrated  and  uncalibrated  stabilization  cases. 

For  both  calibrated  and  uncalibrated  sequences,  the  algorithm  only  needs  to  estimate  three 
rotational  parameters,  and  the  state  vector  x  is  simply  equal  to 

X  =  w  (3.45) 

Based  on  the  simple  kinematic  law  justified  in  the  previous  section,  we  have 


X  =  0  (3.46) 

Subsequently, 

x(ti+i)  =  x(ti)  (3.47) 

This  constitutes  the  plant  equation  in  our  recursive  estimation  algorithm.  We  turn  next  to  the 
observation  equations.  Assume  that  the  tracked  tokens  are  composed  of  M  points  (whose  projection 
points  are  pi, . . . ,  pjv/)  and  N  horizon  lines  (each  one  with  associated  normal  vector  w^,  i  =  1, . . . ,  A 
for  calibrated  sequences,  or  alternatively  with  given  Oj-,  i  =  1, . . . ,  JV  for  uncalibrated  sequences,  with 
Oi  defined  as  in  (3.20)).  The  measurement  vector  in  the  calibrated  case  is  defined  by 

Z  =  (pf,---,PM,wf,...,w^)^  (3.48) 

From  (3.12)  and  (3.17)  we  can  write  the  measurement  equations  as 


z{ti+i)  =  hi+i,;[x(iti+i)]  +  n{ti+i) 


(3.49) 


where  h  is  a  nonlinear  function  while  n  denotes  the  measurement  noise.  More  specifically,  in  the 
calibrated  case,  hj+i_j[x(tj+i)]  is  expressed  as 

r  (c^Pi  +  l)"HApi  +  b)  1 


[x(tj.j.i)]  — 


(c^PM  +  1)  H-A-Pm  +  b) 

(c^viTi  -f  l)"^(Awi  +  b) 


(3.50) 


L  (c^Wiv  +  1)  ^Awiv  -I-  b)  J 

with  A,  b,  and  c  expressed  with  respect  to  the  state  vector  components  as 

_  ^-1  /c^ll  +  /c^l2  +  ^c'>'32  _  ®11  ®12 

fcf'21  +  J/c^Sl  fcf'22  +  yc'f'32  «21  <*22 

]3  _  ^-1  -fcXcni  -  fcycTl2  +  flriz  -  xlrz\  -  XcycTsz  +  fcXcTzZ 

-fcXcr2\  -  /cJ/cJ'22  +  /c  ^23  “  “  J/c^32  +  fcVcr^i 


c  =  d~ 


-1  rzi 


-Xcrsi  -  2/c^32  +  /c’'33 


(3.51) 


(3.52) 
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(3.53) 

(3.54) 


The  measurement  vector  in  the  uncalibrated  case  is  instead  defined  by 

/  T  T  T  T\T 

Z  =  (pj 

and  the  observation  equation  then  uses 

r  (c^pi  +  l)-i(Api  +  b) 


(3.55) 


i,i[x(te+i)] 


(c^PM  +  1)  ^(ApM  +  b) 
(-b^oi  +  l)"^(A^oi  -  c) 

(-b^ojv  +  l)"^(A^o;v  -  c) 


(3.56) 


with  A,  b,  and  c  derived  from  the  identities 


_  J_1  0,22  ~  ^2^2  ^2^1  —  021 

6iC2  —  Oi2  Oil  —  biCi 


_  J-1  O22C1  -  02102 

_  (122^1  —  C^12^2 

aii62  “*  o>2ih 

—  <^11^22  ”  ^12^21 


(3.57) 


(3.58) 


(3.59) 

(3.60) 


With  the  plant  and  measurement  equations  given  in  (3.47)  and  (3.49),  when  horizon  lines  and 
points  are  tracked,  the  EKF  scheme  can  be  applied  to  recursively  estimate  the  two-frame  angular 
velocity.  This  scheme  consists  of  the  following  steps: 

•  Step  1:  State  and  covariance  propagation 

m+i)  =  x(tf) 

S(t-+i)  =  S(tf)  +  S4ti+i) 

where  ±{tf)  and  S(tf)  denote  the  estimates  of  x(ti)  and  the  associated  covariances:  they 
are  obtained  based  on  information  contained  in  the  sequence  up  to  the  frame.  x(<j.).i) 
and  S(tj7i.i),  on  the  other  hand,  are  the  predicted  estimates  of  x(ti+i)  and  the  predicted 
covariances  respectively  before  the  incorporation  of  the  (i-f  1)*^  frame,  while  Siy(tjq.i)  is  the 
covariance  of  the  plant  noise  w(tj+i). 


•  Step  2:  State  and  covariance  update 

K(ti+i)  =  S(t-+i)Hf+i,aHm,iS(tr+i)Hr+i,  +  S„(4+i)]-i 

^(^i+i)  ~  x(t“|.i)  +  K(ti4.i)  |z(ti+i)  -  hi+i,i[x(tj-+i)]|  (3.62) 

S(t+i)  =  [I-K(ti+i)Hi+i,i]S(t-+i) 

where  x(t^i)  is  the  desired  estimate  of  x(tj+i),  and  S(ti+i)  is  the  associated  covariance. 
K(ti+i),  Sn(4+i)  and  I  respectively  represent  the  gain  matrix,  the  covariance  of  n(ti+i)  and 
the  identity  matrix.  on  the  other  hand,  is  the  Unearized  approximation  of  hj+i,i. 


5x(ti+i) 


(3.63) 


lx(q+i) 
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The  algorithm  is  initialized  by  a  batch  process  using  an  LS  estimate  of  the  rotational  parameters 
as  in 

imn  ^  ||z(t,+i)  -  hi4.i,i[x(#j+i)]||2  (3.64) 

1/  ^ 

The  minimization  can  be  solved  using  techniques  based  on  gradient  descent,  such  as  conjugate 
descent. 

This  concludes  our  discussion  of  parameter  estimation.  The  next  section  addresses  image  prim¬ 
itive  tracking.  Synthetic  and  real  experiments  are  reported  as  well. 

3.6  Experimental  Results 

We  first  briefly  describe  approaches  to  the  detection  and  tracking  of  horizon  lines  and  points. 
Experimental  results  on  two  real  image  sequences  are  then  presented. 

3.6.1  Detection/Tracking  of  Image  Primitives 

The  first  set  of  visual  cues  for  characterizing  the  rotation  consists  of  horizon  lines.  There  have  been 
numerous  approaches  to  tracking  a  set  of  fine  segments  over  a  sequence  [11,  60].  For  simplicity, 
we  only  focus  on  tracking  one  fine  in  our  work,  although  using  a  set  of  lines  is  desirable  in  some 
situations. 

Assuming  that  the  lines  near  the  horizon  appear  in  the  form  of  large  vertical  brightness  deriva¬ 
tives,  the  detection  of  the  line  of  interest  is  achieved  in  three  steps: 

•  Step  1:  Based  on  the  histogram  of  the  image,  a  binary  image  is  generated  by  thresholding 
the  original  image. 

•  Step  2:  In  the  binary  image,  the  pixels  with  large  vertical  brightness  derivatives  most  likely 
constitute  points  on  the  horizon  fine.  A  simple  template  matching  technique  is  applied  to 
identify  pixels  with  large  brightness  derivatives. 

•  Step  3:  A  median  fit  method  is  used  to  robustly  group  identified  pixels  into  Unes.  The 
longest  line  is  taken  to  be  the  line  of  interest. 

In  the  experiments,  the  above  procedures  are  applied  to  each  image  in  the  sequence.  The  line 
extracted  from  each  image  is  assumed  to  be  near  the  horizon,  and  these  fines  are  matched  to  each 
other. 

The  second  class  of  inputs  to  the  full  stabilization  scheme  are  the  image  plane  trajectories  of  a 
set  of  points  near  the  horizon.  The  localized  tracking  algorithm  described  in  Chapter  5  is  employed 
to  obtain  feature  point  trajectories  and  is  not  repeated  here.  However,  in  our  formulation,  it  is  not 
required  that  the  same  set  of  points  be  used.  Therefore,  we  only  need  to  focus  on  finding  matching 
points  between  two  frames.  A  different  set  of  points  can  be  used  whenever  this  is  desirable. 

3.6.2  Synthetic  Experiments 

This  section  tests  the  results  of  Section  3.4;  we  illustrate  the  performance  of  the  kinematic  law 
using  a  one- dimensional  example,  and  show  that  both  laws  yield  comparable  performance  when 
sufficient  number  of  visual  observations  are  present.  Consider  the  scenario  in  which  the  vehicle 
travels  along  a  straight  path  most  of  the  time  except  when  encountering  impulse-like  disturbances 
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(bumps).  Each  bump  is  modeled  by  a  half  sine  wave  with  a  given  height  and  width.  (For  simplicity, 
no  smooth  rotational  motion  during  driving  is  assumed.)  The  excitations  to  the  vehicle  are  then 
given  by 

bhSm[^{vt-zo)]  f<t<^ 

0  elsewhere 


a;oi(i)  = 


with 


2^02(0  = 


2^03(0  =  ®oi 


2'04(0  —  ^02 


where  zq,  L,  and  v  respectively  denote  the  location  of  the  bump,  the  vehicle’s  length  and  its  forward 
speed.  (Only  one  bump  is  used  in  this  example,  with  Zq  =  1.345  m,  bh  =  0.1  m,  byj  =  0.2  m,  i  =  2.7 
m  and  v  =  13.45  m  •  s“^).  The  nominal  values  of  the  vehicle  parameters  are  hsted  in  Table  3.1,  and 
the  oscillatory  motion  is  synthesized  according  to  the  behavior  of  the  model,  derived  in  Appendix  C, 
with  only  the  pitching  motion  shown  in  Figures  3.5a  and  3.5b. 

During  driving,  a  sequence  of  images  is  acquired  aX  20  Hz  from  an  on-board  camera  (each  image 
has  size  2.0  x  2.0  and  resolution  2000  x  2000),  and  a  set  of  distant  points  is  tracked;  the  coordinates 
of  these  points  with  respect  to  the  initial  camera  frame  of  reference  are  listed  in  Table  3.2. 


Table  3.1:  Model  parameters.  Mb'  the  mass  of  the  sprung  element,  lyyZ  the  moment  of  inertia 
along  the  pitch  axis,  Wa  and  Wb'-  the  distance  of  the  center  of  gravity  to  the  front  and  rear  ends. 


Mb 

^yy 

M^r 

Kt 

1710.0kg 

1031.3kg-m^ 

57.5kg 

75.0kg 

200.0kN-m-^ 

Kf 

Kr 

Cf  {Cr) 

Wa 

Wb 

18.0kN-m-^ 

lO.OkN-m-i 

1.0kN-m“^-s“^ 

1.4m 

1.3m 

Table  3.2:  Distant  point  locations. 


Point 

3D  coordinates 

1 

84 

-84  1200 

2 

160 

-80  2000 

3 

170 

115  2300 

4 

100 

140  1750 

Subsequently,  to  compare  the  kinetic  and  and  kinematic  law  based  estimators,  we  design  a 
recursive-type  estimator  similar  to  the  one  used  in  the  real  application  to  estimate  the  pitching 
motion.  The  resulting  bias  and  Root- Mean- Squared  Error  (RMSE)  in  the  estimates  of  the  attitude 
change  between  t  and  t  +  At,  6d{t)  =  6(t)  -  0{t  -  At),  are  shown  in  Figures  3.5c  and  3.5d.  As 
seen  from  the  figures,  when  reliable  image  cues  are  available,  the  performance  of  the  kinematic-law 
based  estimator  (dashed  line)  is  very  close  to  that  of  the  kinetic-law  based  one  (solid  line).  More 
importantly,  the  larger  bias  in  the  kinematic-law  based  estimator,  due  to  the  modeling  error  in  the 
time  evolution  of  the  parameters,  is  negligible. 
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(c)  (d) 

Figure  3.5:  Kinetic  versus  kinematic:  (a)  (b)  0{t),  (c)  Bias  in  the  estimates  of  Od{t)^  (d)  RMSE 

in  the  estimates  of  Od{t)- 

3.6.3  Real  Imagery 

We  show  the  results  of  the  application  of  the  stabilization  schemes  to  two  sequences  with  significant 
unstabilized  components  in  both  calibrated  and  uncalibrated  cases.  To  let  the  reader  more  precisely 
judge  the  results  yielded  by  this  technique,  we  have  placed  MPEG  movies  showing  the  original 
sequences,  the  primitive  tracking  and  the  stabilized  sequences  at  the  following  URL: 

http:  /  /  www.cfar.umd.edu/  ~  yao /stabilization,  html. 

They  can  easily  be  viewed  using  any  web-browser  (Mosaic,  NetScape,  etc.). 

Martin  Marietta  Sequence  The  first  sequence,  distributed  by  Martin  Marietta,  was  obtained 
from  a  vehicle  performing  off-road  navigation.  Figure  3.6  shows  four  frames  from  the  sequence; 
each  frame  has  size  347  x  238,  The  motion  is  composed  of  translation  (with  a  dominant  looming 
component)  and  unstabilized  rotation.  The  FOV  is  40  X  30  degrees  (in  the  horizontal  and  vertical 
directions,  respectively),  while  the  optical  axis  intersects  the  image  plane  at  the  center  of  the  image. 
The  experiments  were  first  carried  out  for  the  calibrated  case. 

In  this  sequence,  both  the  mountain  profile  and  the  line  near  the  horizon  are  detected.  Fig- 
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(c)  (d) 

Figure  3.6:  Martin  Marietta  sequence:  (a)  Image  9,  (b)  Image  39,  (c)  Image  74,  (d)  Image  99. 

ure  3.7a  shows  the  results  for  one  frame.  (The  mountain  profile  is  not  employed  by  the  current 
algorithm.)  Four  feature  points  close  to  the  detected  line  are  identified  and  tracked  over  the  se¬ 
quence  afterwards.  For  display  purposes,  the  resulting  image  plane  trajectories  are  superimposed 
on  the  last  frame  of  the  sequence  as  shown  in  Figure  3.7b.  The  LS  estimate  of  the  rotation  is  also 
computed  as  if  only  the  horizon  line  were  available.  Subsequently,  stabilization  from  both  full  and 
LS  rotation  is  applied;  the  recursive  algorithm,  in  addition,  uses  the  estimates  from  the  batch  algo¬ 
rithm  computed  from  the  first  ten  frames.  Figures  3.7c  and  3.7d  respectively  show  the  LS  and  full 
angular  velocity  estimates.  The  solid  line,  the  dashed  line  and  the  dashed-dotted  line  respectively 
correspond  to  pitch  motion,  yaw  motion  and  roll  motion  respectively.  Since  the  vehicle  exhibits 
very  small  lateral  motion  and  the  sequence  is  densely  sampled,  the  LS  estimates  are  therefore  close 
to  the  fully  stabilized  estimates  for  this  sequence.  Finally,  a  stabilized  sequence  is  obtained  using 
the  global  stabilization  scheme  with  the  full  rotational  estimates.  The  stabilized  sequence  appears 
to  undergo  translation  only,  with  the  direction  of  translation  varying  noticeably  when  the  rotational 
component  in  the  original  sequence  is  large. 

The  stabilization  scheme  for  the  uncalibrated  case  was  also  applied,  assuming  that  the  intrinsic 
parameters  are  unknown.  The  sensitivity  of  the  eight  parameters  with  respect  to  the  four  intrin¬ 
sic  parameters,  obtained  using  the  calibrated  parameters  as  nominal  values,  are  first  shown  in 
Figures  3.8  and  3.9.^  (The  additional  parameter  is  due  to  the  different  focal  length  in  either  direc¬ 
tion.)  For  evaluation  purposes,  we  only  illustrate  the  sensitivity  of  each  parameter  with  respect  to 

^Instead  of  showing  the  computed  J,  Figures  3.8  and  3.9  show  the  relative  sensitivity,  defined  as  J  •  A,  with  A  a 
diagonal  matrix  whose  elements  are  Ao* 


(C)  (d) 

Figure  3.7;  Experimental  results  from  the  calibrated  scheme  for  Martin  Marietta  sequence:  (a)  The 
detected  line  and  the  mountain  profile,  (b)  Feature  point  trajectories,  (c)  LS  estimates,  (d)  Fully 
stabilized  estimates. 

the  focal  length  in  the  horizontal  direction  and  Xc. 

As  discussed  earlier,  these  values  are  small  and  therefore  a  reasonable  deviation  from  the  nominal 
values  of  the  intrinsic  parameters  is  not  critical.  Consequently,  we  vary  the  parameters  and  perform 
different  tests.  For  example,  assume  the  focal  length  in  pixels  to  be  700  and  600  in  the  horizontal 
and  vertical  directions  respectively  (the  real  values  being  480  and  443  respectively),  and  the  image 
center  to  be  50  pixels  away  from  the  true  position,  say  (100,  100).  Then  using  the  same  visual 
cues.  Figure  3.10  shows  the  estimated  angular  velocity.  The  uncalibrated  stabilization  scheme  is 
thereafter  applied  to  the  original  sequence.  It  can  be  seen  from  the  MPEG  movie  that  the  result 
is  close  to  the  one  obtained  using  the  calibrated  scheme.  The  result  for  the  uncalibrated  sequence 
using  unconstrained  projectivity  parameters,  i.e.  direct  estimation  of  the  eight  parameters  from  the 
image  features,  leads  instead  to  very  poor  results,  since  the  estimation  is  unstable. 


NIST  Sequence  The  same  procedure  wass  applied  to  another  sequence,  provided  by  NIST, 
which  Wets  also  acquired  from  an  off-road  vehicle.  Figure  3.11  shows  four  frames  of  the  sequence  in 
which  each  frame  is  of  size  640  x  480.  The  motion  is  composed  of  a  translation  with  steering  and 
unstabilized  motion  components.  The  FOV  is  70  and  60  degrees  along  the  horizontal  and  vertical 
directions  respectively,  while  the  optical  axis  again  intersects  the  image  plane  at  the  center  of  each 
image. 
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(a) 


Figure  3.8:  Sensitivities  of  A  with  respect  to 
(dashed  line)  for  Martin  Marietta  sequence:  (a) 


(b) 


c  in  the  horizontal  direction  (solid  line)  and  Xc 
*11,  (b)  ai2,  (c)  021,  (d)  022- 


A  line  and  partial  object  profiles  near  the  horizon  are  detected  as  displayed  in  Figure  3.12a, 
while  the  feature  trajectories  are  similarly  superimposed  on  the  last  frame  as  shown  in  Figure  3.12b. 
The  LS  and  full  estimates  of  the  angular  velocity  are  plotted  in  a  similar  fashion  in  Figures  3.12c 
and  3.12d  respectively.  Since  the  sequence  exhibits  some  steering,  the  LS  and  fuU  estimates  are 
quite  different.  In  addition,  the  steering  behavior  is  preserved  in  the  stabilized  sequence;  only  the 
unstabilized  motion  is  removed. 

We  also  applied  the  uncalibrated  stabihzation  scheme  to  this  sequence.  Again,  we  choose  the 
focal  length  in  pixels  to  be  600  and  700  (in  contrast  with  the  true  values  249  and  457),  and  the 
center  of  the  image  to  be  (250,  300).  The  sensitivity  of  the  projective  coefficients  is  shown  in 
Figures  3.13  and  3.14.  The  angular  velocity  estimates  are  illustrated  in  Figure  3.15.  The  MPEG 
movie  again  shows  good  stabilization  results  for  this  sequence. 

3.7  Conclusion 

This  chapter  has  presented  a  scheme  for  stabilizing  a  sequence  acquired  by  a  moving  observer.  In 
particular,  the  algorithm  exploits  the  temporal  information  in  the  sequence  and  utilizes  various 
visual  cues.  Image  warpings  derived  from  the  3D  motion  are  used  instead  of  other  approximate  2D 
mappings,  thereby  capturing  more  closely  the  image  motion  resulting  from  the  camera  rotation. 
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(C)  (d) 

Figure  3.9:  Sensitivities  of  b  and  c  with  respect  to  fc  in  the  horizontal  direction  (solid  line)  and  Xc 
(dashed  line)  for  Martin  Marietta  sequence:  (a)  bi,  (b)  b2,  (c)  ci,  (d)  C2. 

The  consideration  of  global  stabilization,  in  addition,  makes  the  algorithm  more  suitable  to  real 
applications.  We  analyze  the  nature  of  the  resulting  sequence.  The  use  of  kinetic  laws  for  esti¬ 
mation  purposes  is  carefully  justified  and  tested  in  synthetic  cases.  Lastly,  we  study  the  removal 
of  rotational  motion  from  uncalibrated  sequences,  for  which  a  scheme  to  alleviate  the  unstable 
estimation  of  the  projectivity  coefficients  is  proposed  and  tested.  Ah.  results  have  been  thoroughly 
tested  and  the  resulting  sequences  have  been  made  available  to  the  interested  reader. 
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(a) 


(b)  (c) 

Figure  3.10:  Angular  velocities  from  the  uncahbrated  scheme  (solid  line)  versus  calibrated  scheme 
(dashed  line)  for  Martin  Marietta  sequence:  (a)  (b)  ojy,  (c)  Wj. 


(C)  (d) 

Figure  3.11:  NIST  sequence:  (a)  Image  10,  (b)  Image  35,  (c)  Image  60,  (d)  Image  80. 
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(c)  (d) 

Figure  3.12:  Experimental  results  from  the  calibrated  scheme  for  the  NIST  sequence:  (a)  The 
detected  line  and  partial  object  profiles,  (b)  Feature  point  trajectories,  (c)  LS  estimates,  (d)  Fully 
stabilized  estimates. 
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(c)  (d) 

Figure  3.13:  Sensitivities  of  A  with  respect  to  fc  in  the  horizontal  direction  (solid  line)  and  x 
(dashed  line)  for  NIST  sequence:  (a)  an,  (b)  ai2,  (c)  021,  (d)  022- 
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Figure  3.14:  Sensitivities  of  b  and  c  with  respect  to  fc  in  the  horizontal  direction  (solid  line)  and 
Xc  (dashed  line)  for  NIST  sequence:  (a)  bi,  (b)  62?  (c)  ^i,  (d)  C2. 


(a) 


(b)  (c) 

Figure  3.15:  Angular  velocities  from  the  uncalibrated  scheme  (solid  line)  versus  calibrated  scheme 
(dashed  line)  for  NIST  sequence:  (a)  uixi  (b)  ^yi  (c) 


Chapter  4 


Structure  from  Selective  Stabilization 


4.1  Introduction 

We  address  the  interpretation  of  visual  motion  from  selective  stabilization  in  this  chapter.  The  con¬ 
sideration  of  oscillatory  motion  is  important  for  many  navigation  related  applications.  For  example, 
for  an  active  vision  system,  the  separation  of  residual  oscillatory  motion  from  smooth  motion  is 
useful  for  achieving  fixation.  Another  example  where  selective  stabilization  is  important  is  teleoper¬ 
ation  in  which  the  vehicle  needs  to  be  remotely  controlled.  An  image  sequence  unperturbed  by  the 
residual  oscillatory  motion,  while  stiU  preserving  smooth  motion,  is  highly  desirable:  the  teleopera¬ 
tor  needs  to  fully  evaluate  the  effects  of  climbing  and  steering.  Furthermore,  while  an  approximate 
selective  stabilization  scheme  could  be  attempted  by  simple  low-pass  filtering  of  the  computed  total 
rotational  components,  a  scheme  which  more  closely  resembles  mechanical  stabilization  is  of  more 
interest. 

To  achieve  selective  stabilization,  we  first  consider  the  recovery  of  parameters  relevant  to  total 
rotation  which  is  the  sum  of  smooth  rotation  and  oscillatory  rotation.  This  procedure  has  been 
described  in  the  previous  chapter.  Subsequently,  we  seek  out  the  separation  of  smooth  rotation 
and  residual  oscillatory  rotation.  The  essence  is  the  exploitation  of  appropriate  dynamic  laws  for 
both  phenomena.  By  assuming  the  desired  smooth  rotation  results  in  smooth  attitude  change  of 
a  moving  vehicle,  a  kinematic  law  is  employed  to  model  the  behavior  of  corresponding  smooth 
rotation  parameters.  The  residual  oscillatory  rotation  is  captured  by  a  kinetic  model.  A  four- 
wheel  vehicle  model  described  in  the  previous  chapter  is  considered  for  this  purpose.  Moreover, 
it  is  observed  that,  on  some  occasions,  the  smooth  rotation  and  the  residual  oscillatory  rotation 
are  not  completely  coupled.  If  the  high  frequency  yaw  motion  is  negligible,  steering  is  directly 
characterized  by  total  rotation.  Similarly,  if  the  banking  behavior  is  not  considered,  the  roll  motion 
is  also  observable  from  the  total  rotation.  Therefore,  only  the  behavior  of  climbing  and  pitch 
motion  is  always  coupled.  However,  the  climbing  phenomenon  does  not  exist  at  all  times.  It  only 
lasts  during  the  short  period  whenever  the  vehicle  moves  between  surfaces  with  different  slopes,  for 
example,  from  a  horizontal  surface  to  an  uphill  path.  Consequently,  we  assume  that  only  climbing 
and  pitch  phenomena  need  to  be  separated  in  our  selective  stabilization  scheme.  A  detection  scheme 
is  then  designed  for  the  detection  of  start  and  end  of  climbing.  Based  on  the  detection  results,  the 
algorithm  employs  dynamic  laws  to  achieve  selective  stabilization. 

Subsequently,  we  estimate  the  relative  positions  of  close  scene  points  with  respect  to  the  vehicle. 
Much  of  the  earlier  work  estimates  these  relative  positions  without  consideration  of  the  residual 
oscillatory  rotation.  Decisions  or  actions  based  on  the  resulting  estimates  are  likely  to  be  inappro¬ 
priate.  With  selective  stabilization  achieved,  we  estimate  the  3D  locations  of  a  set  of  close  features 
in  a  more  desirable  frame  of  reference,  which  is  less  perturbed  by  the  oscillatory  rotation. 
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The  organization  of  this  chapter  is  as  follows.  Section  4.2  discusses  the  separability  of  smooth 
rotation  and  residual  oscillatory  rotation.  The  motion/structure  recovery  algorithm  including  the 
detection  of  the  start  and  end  of  smooth  rotation  as  well  as  estimation  of  structure  parameters 
is  presented  in  Section  4.3.  Section  4.4  reports  results  of  synthetic  experiments.  Conclusions  are 
given  in  Section  4.5. 

4.2  Selective  Stabilization 

The  inherent  nature  of  video  signal  processing  implies  that  temporal  information  present  in  the 
sequence  should  be  exploited.  We  show  that  suitable  dynamic  laws  also  facilitate  selective  stabi¬ 
lization  in  this  section. 

For  convenience,  denote  the  angular  velocities  arising  from  the  smooth  rotation  by  Ug  and  from 
the  oscillatory  rotation  by  oJus  respectively.  Consider  first.  Since  the  smooth  rotation  is  due 
to  steering  and  climbing,  it  is  assumed  that  the  resulting  attitude  changes  smoothly  over  time.  A 
simple  kinematic  law  is  therefore  employed  to  describe  the  behavior  of  :  {oJsx^(^sy,‘^szV ^ 

<^s  =  0  (4.1) 

On  the  other  hand,  recall  the  four-wheel  vehicle  model  described  in  the  previous  chapter.  The 
residual  oscillatory  rotation  x„s  is  described  by  (3.34),  i.e. 

—  US  “I”  (4.2) 

Instead  of  using  sensors  such  as  accelerometers  to  obtain  u^j,  we  employ  an  approximate  law  to 
describe  as 

^U$  ~  ^  US^  US  (4.3) 

It  is  shown  in  Appendix  D  that  this  approximation  introduces  an  additional  error  in  separating 
smooth  rotation  and  residual  oscillatory  rotation.  However,  the  error  remains  bounded. 

Next,  define  the  stabilized  coordinate  system  V5(^)  whose  orientation  is  determined  by  the 
smooth  rotation,  and  whose  center  is  located  at  the  center  of  gravity  of  the  vehicle.  Consider 
also  the  unstabilized  coordinate  system  V(t),  whose  attitude  is  in  addition  affected  by  the  residual 
oscillatory  rotation,  and  with  its  origin  shifted  from  the  center  of  gravity  due  to  bounce.  Then, 
from  two  images  acquired  at  times  t  —  At  and  we  observe  the  total  angular  velocity  accounting 
for  the  net  attitude  change  between  V{t  —  At)  and  V(t).  In  other  words,  if  R(a;;  t,  t  —  At)  aligns  the 
coordinate  system  V(t  ~  At)  with  V(t),  R5(a?5;t,t  -  At)  takes  into  account  the  attitude  difference 
between  V5(t  -  At)  and  V5(t),  and  Kus{0j<j>]t)  accounts  for  the  additional  attitude  due  to  the 
oscillatory  rotation  at  time  t, 

1  -e{t)  ■ 

Tius{0,<l>;t)=  -<^(t)  1  0  (4.4) 

^(t)  0  1 

then  it  can  be  shown  that 

R{u-,t,t-At)  =  (4.5) 

Subsequently,  assume  that  R  and  can  be  approximated  as 

R(a>;t,t-At)  w  l-HAt 
Rs{(jiJs]'t,t  —  At)  ^  I—HgAt 
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(4.6) 

(4.7) 


with 


Also, 


n  = 


0 


-U, 


y 


-^Z 

0 

Wrr 


Uy 

-Wx 


and 


At 

At 


0 

-OJsz 

OJsy 

Qs  = 

<^$z 

0 

~^sx 

(4.8) 

m 

ki) 

^sx 

0 

(4.9) 

Then  the  total  angular  velocity  oj  is  related  to  the  angular  velocity  corresponding  to  the  smooth 
rotation  and  residual  oscillatory  rotation  by 


a>  =  ojs  +  oJi, 


(4.10) 


with 

=  (4.11) 

Since  the  angular  velocity  component  of  the  yaw  motion  is  assumed  to  be  zero,  we  only  focus  on 
the  separabihty  of  the  other  two  phenomena,  i.e.  chmbing  and  pitch  as  weU  as  banking  and  roll. 
Using  the  dynamic  laws  in  (4.1)  and  (4.3),  the  following  linear  system  can  be  constructed: 


f  xi  =  Gxi 
I  oJi  =  Fxi 


where  xi  =  {u)sy,<^sz,^ls)^ ^  ■> 


G  = 


02x2  02x4 

04x2 


(4.12) 


(4.13) 


with  02x2?  02x4  04x2  being  zero  matrices  of  conformable  dimensions. 


10  0  10  0 
0  1  0  0  0  1 


(4.14) 


The  separability  of  smooth  rotation  and  residual  oscillatory  rotation  can  now  be  verified  by  checking 
the  rank  of  the  observabihty  matrix  O, 


F 

FG 

FG® 


(4.15) 


Except  in  some  special  cases  in  which  the  matrix  is  singular,  O  in  general  has  full  rank.  There¬ 
fore,  by  employing  different  dynamic  laws  to  describe  the  smooth  rotation  and  residual  oscillatory 
rotation,  it  is  possible  to  separate  the  two  rotation  phenomena. 

The  next  section  presents  schemes  for  the  detection  of  the  start  and  end  of  smooth  rotation, 
and  the  estimation  of  3D  locations  of  close  feature  points  in  the  stabiUzed  coordinate  system. 
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4.3  Parameter  Estimation 

The  incorporation  of  appropriate  dynamic  laws  to  describe  smooth  rotation  and  residual  oscillatory 
rotation  has  made  selective  stabilization  possible.  Since  smooth  rotation  does  not  last  for  a  long 
time,  the  detection  of  the  beginning  or  end  of  smooth  rotation,  especially  climbing,  facilitates  the 
task  of  selective  stabilization.  In  addition  to  the  total  rotational  parameter  estimation  scheme 
mentioned  in  Section  3.5,  this  section  describes  the  other  schemes  which  comprise  our  algorithm 
for  off-road  vehicular  navigation. 

4.3.1  Maneuver  Detection  /  Selective  Stabilization 

As  discussed  earlier,  when  the  high  frequency  yaw  motion  is  negligible,  steering  is  directly  known 
from  the  total  angular  velocity.  Similarly,  the  phenomenon  of  banking  only  lasts  for  short  periods 
and  more  importantly,  it  does  not  have  later  effects  on  the  direction  in  which  the  vehicle  travels. 
Banking  is  therefore  also  neglected,  and  consequently,  the  angular  velocity  component  explaining 
roll  motion  is  obtained  directly.  However,  climbing  is  not  negligible;  the  vehicle  generally  moves 
along  a  direction  resulting  from  climbing.  Thus,  the  separation  of  climbing  and  pitch  motion  is 
important.  We  first  describe  a  detection  scheme  designed  to  detect  the  beginning  and  end  of  the 
climbing  phenomenon. 

Recall  the  image  motion  expressed  in  (3.4).  When  a  distant  point  is  considered,  its  vertical 
movement  is  described  by 

X  =  -  Xc){y  -  Vc)]  -  OJy[fc  +  C®  “  X^f]  -|-  U:,{y  -  y^)  (4.16) 

if  the  corresponding  projection  point  is  close  to  the  vertical  axis,  i.e.  y  -  t/c  »  0,  then 

X  «  -iOylfc  -I-  (a;  -  XcY]  (4.17) 

In  other  words,  the  vertical  movements  of  such  features  are  dominantly  affected  by  climbing  and 
pitch  motions.  Since  a  zero  order  dynamic  law  is  assumed  for  the  total  angular  velocity,  it  is 
expected  that  when  the  vehicle  performs  maneuvers  (i.e.  starts  climbing  or  finishes  climbing), 
large  errors  occur  at  these  instants  in  predicting  the  vertical  movements  of  corresponding  points. 
We  define  [zm]x  to  be  the  vector  consisting  of  the  vertical  coordinates  of  N  such  feature  points, 
Pml  ?  •  •  •  ?PmiV 5 

[Zmlx  “  ([Pml]x?  *  •  •  9  [PmA/‘]a:)  (4.18) 

and  the  test  statistic  6  at  time  t  as 

6{t)  =  -  [z^(t)]x)^S”^(0([z7n(0]^  ”  [Zm{t)]x)  (4.19) 

where  z^(t)  denotes  the  predicted  locations  of  these  N  feature  points  at  time  t;  its  element  is 
computed  by 

Pm(0  =  [c^Pm(<  -  At)  -h  l]"^[Ap„^(t  -  At)  -f  b]  (4.20) 

where  the  predicted  angular  velocity  is  used  in  calculating  A,b,c.  S77^(t),  on  the  other  hand,  is 
the  covariance  matrix  of  the  innovation  vector  [zm(t)]a:  -  [zm{t)]x*')  it  can  easily  be  computed  in  our 
EKF  formulation.  Then,  based  on  <5(t),  we  declare  that  the  vehicle  undergoes  maneuvers  at  i  —  At 
when 

6{t)  >  TH  (4.21) 
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with  TH  a  threshold  value.  The  types  of  maneuvers  corresponding  to  the  beginning  or  end  of 
climbing  are  then  determined.  If  the  vehicle  starts  climbing  at  a  rate  Usy{t)  much  larger  than  the 
pitching  velocity  6{t),  the  resulting  maneuver  can  be  identified  by  comparing  6{t)  to  previous  non¬ 
maneuver  test  statistics  S{t  -  At),  6{t  -  2At),  mAt),  with  m  the  size  of  a  moving  window. 

Similarly,  if  the  type  of  maneuver  leads  to  the  end  of  the  climbing  phenomenon,  the  scheme  is  able 
to  react  instantaneously. 

There  are  situations  in  which  the  ojsy{i),  at  maneuver  instants,  is  less  than  6{t).  If  we  regard 
Usyit)  as  the  desired  signal,  and  9(t)  as  the  additive  noise,  we  are  dealing  with  a  problem  involving 
a  small  signal  to  noise  ratio.  The  test  statistic  6{t)  may  become  inappropriate  since  the  maneuver 
test  statistic  does  not  manifest  itself  in  larger  values.  We  therefore  impose  another  conservative 
criterion.  This  criterion  cannot  detect  maneuvers  instantaneously.  However,  it  notices  the  event  a 
few  frames  later  and  prevents  the  divergence  of  the  whole  algorithm. 

Specifically,  observe  that  when  the  vehicle  only  undergoes  pitch  motion,  p^i, . . . ,  PmN  are  likely 
to  oscillate  around  their  own  reference  positions.  The  conservative  detection  criterion  monitors  the 
vertical  movements  of  these  points  with  some  frequency  less  than  the  sequence  acquisition  rate. 
When  these  N  points  start  to  move  away  from  the  reference  positions,  the  algorithm  reports  the 
start  of  climbing.  Reprocessing  a  small  set  of  images  is  then  performed.  Afterwards,  when  these 
points  oscillate  around  new  reference  positions,  the  climbing  is  assumed  to  end. 

We  now  proceed  to  address  the  separation  of  cMmbing  and  pitch  motion.  First,  let  the  vector 
y  consist  of  the  quantities  of  interest.  When  the  start  of  climbing  is  detected. 


(4.22) 

and  otherwise. 

11 

(4.23) 

Next,  we  use  a  Kalman  Filter  (KF)  to  achieve  selective 
obtained  by  employing  the  dynamic  laws  described  in  (4.1) 
can  be  written  as 

Uy  =  Hy 

stabilization.  The  plant  equation  is 
and  (4.3).  The  observation  equation 

(4.24) 

where  when  Usy  is  included, 

H  =  [1  0  1] 

(4.25) 

and  otherwise, 

H  =  [0  1] 

(4.26) 

Using  the  assumptions  that 

f  Usj;  =  Ux 

1  =  Wz 

(4.27) 

the  smooth  rotation  and  the  residual  oscillatory  rotation  are  separated.  The  next  section  discusses 
the  use  of  selective  stabilization  to  recover  the  3D  locations  of  feature  points  which  are  close  to  the 
vehicle. 


4.3.2  Structure  from  Selective  Stabilization 

Recall  that  the  motion  of  a  close  3D  point  relative  to  the  stabilized  coordinate  system  is  described 

by 

=  -u:,  X  Pv,  -  V,  (4.28) 
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with  Pvs  denoting  the  coordinates  of  the  point  in  V*,  being  the  angular  velocity  due  to  the 
smooth  rotation,  and  Vs  being  the  vehicle’s  translational  velocity  under  no  disturbances.  When 
the  vehicle  undergoes  constant  smooth  rotation  and  translation  between  t  —  At  and  t,  we  have 

Pv,(t)  =  -  At)Pvs{t  -  At)  +  Ts{(j>}s',t,t-  At)  (4.29) 

where  Rs  accounts  for  the  resulting  attitude  change  as  explained  in  (4.7),  while  can  be  shown 
[60]  to  be 


Ts{iMs-,t,t-  At)  = 


lAt 


1  -  cos(|ws|At)  |ws|At  -  sindw^jAt) 


1^5 


■f2s  + 


V. 


(4.30) 


with  f2s  being  defined  in  (4.8)  and  |a>s|  denoting  the  magnitude  of  uPg-  In  particular,  observing 
that  the  translational  movement  of  a  vehicle  is  along  its  longitudinal  axis  most  of  the  time  [33], 
is  simplified  to 

V,  =  (0,0,u,)^  (4.31) 

with  Vs  being  the  forward  speed.  Since  Vg  can  easily  be  obtained  from  the  odometer,  we  assume 

that  it  is  known. 

The  relative  motion  of  the  point  with  respect  to  the  unstabilized  coordinate  system  V  is  then 
obtained  as 

Pv(t)  =  R„.(^,  t)  {Pvsit)  -  T„.(i)}  (4.32) 

where  T^s  is  due  to  the  bounce  of  the  vehicle, 

c* 

T„.(t)  =  (xc(t),0,0)^  (4.33) 


The  projection  of  the  3D  point  on  the  image  is  then  computed  by  applying  the  perspective  projection 
operator  V, 

P  =  ■P(Pv  -  d)  +  Pc  (4.34) 

where  d  specifies  the  position  of  the  camera  with  respect  to  V;  it  remains  constant  because  the 
camera  is  mounted  rigidly  on  the  vehicle.  Since  the  effect  of  is  usually  too  small  to  be  observed 
from  the  images,  it  is  not  considered.  Therefore,  by  tracking  the  points  of  interest  over  the  sequence 
as  well  as  using  the  estimates  from  selective  stabilization,  we  employ  another  EKF  to  estimate  Pv^. 
The  corresponding  plant  equations  for  Mg  such  points  are 

Pv„j(«i+i)  =  Rc(uJc;ti+i,ti)Pv.,i(ii)  +  Tc(wc;ti+i,ti)  j  =  l---Mg  (4.35) 

The  measurement  equations,  on  the  other  hand,  are 

Pj  =  P(Pv,j  -  d)  +  Pc  j  =  l---Mg  (4.36) 

The  undesired  oscillatory  phenomena,  when  estimated  with  respect  to  the  unstabilized  coordinate 
system  V,  are  removed. 

This  concludes  our  scheme  for  off-road  navigation  using  selective  stabilization.  The  next  section 
presents  simulation  results. 
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4.4  Synthetic  Experiments 

In  this  section,  we  illustrate  the  proposed  approach  through  synthetic  experiments.  Two  exper¬ 
iments  were  performed.  One  corresponds  to  the  case  of  high  signal  to  noise  ratio  in  which  the 
attitude  change  due  to  climbing  is  much  larger  than  the  change  arising  from  pitch  motion.  The 
other  one  deals  with  the  case  involving  low  signal  to  noise  ratio. 

Consider  the  scenario  in  which  the  vehicle  undergoes  steering  for  the  first  one  second,  with 
a  rate  of  =  0.1  rad  •  s'^  Immediately  after  the  steering,  the  vehicle  starts  cUmbing  over  a 
hill.  The  smooth  rotation  due  to  climbing  exists  for  two  seconds.  The  climbing  rates  are  set  to 
ojsy  =  0.1  rad-s"^  and  0.01  rad-s"S  in  the  high  and  low  signal  to  noise  ratio  situations  respectively. 
Finally,  the  vehicle  moves  uphill  along  a  straight  path  for  the  last  two  seconds,  i.e.  no  smooth 
rotation  occurs.  The  forward  speed  of  the  vehicle  is  maintained  at  Vs  =  10.0m  •  s  . 

The  surface  irregularities,  on  the  other  hand,  are  modeled  using  first  order  Markov  processes; 
their  coefficients  depend  on  the  terrain  roughness  and  the  vehicle’s  forward  speed.  The  excitations 
to  the  vehicle  are  given  by 

xoi(t)  =  -avxoi{t)  -1-  nav{t)  i  =  1  •  •  -  4  (4-37) 

where  Uav  is  the  white,  Gaussian  driving  noise  with  mean  zero  and  variance  2<T^au.  The  values  of  a 
and  C7  vary  for  different  surfaces.  We  choose  a  =  0.012m  and  a  =  O.Sm-^  [37].  The  nominal  values 
of  the  vehicle’s  parameters  are  listed  in  Table  4.1.  According  to  the  equations  of  motion  derived 
in  Appendix  A,  we  synthesize  the  oscillatory  behavior  of  the  vehicle  and  illustrate  it  in  Figure  4.1 
(only  pitch  and  roU  phenomena  are  shown).  Together  with  the  smooth  motion,  the  behavior  of  the 
vehicle  is  obtained. 

Table  4.1:  The  nominal  parameter  values  of  the  four-wheel  vehicle  model. 


Mb 

lyy 

Izz 

1710.0kg 

1031.3kg -m^ 

201.8kg  •  m^ 

57.5kg 

]\dyjr 

Kt 

Cf  {Cr) 

Kf 

75.0kg 

200.0  kN  -m"^ 

1.0  kN  -m"^  •  s“^ 

18.0  kN  •  m"^ 

Kr 

Ts^r  {Ts,l) 

Wa 

Wb 

10.0  kN  •  m“^ 

0.6m 

1.4m 

1.3m 

In  addition,  during  driving,  a  sequence  of  images  is  acquired  at  20172  from  an  on-board  camera 
(each  image  has  size  2.0  x  2.0  and  resolution  2000  X  2000),  while  a  set  of  features  including  eight 
distant  points  and  four  close  points  are  tracked.  The  coordinates  of  these  points  with  respect  to 
the  initial  frame  of  reference  are  listed  in  Table  4.2. 

Table  4.2:  The  initial  3D  locations  of  the  feature  points:  the  first  eight  points  correspond  to  distant 
features,  while  the  last  four  are  considered  to  be  close  features. 


Point 

3D  coordinates 

Point 

3D  coordinates 

Point 

3D  coordinates 

1 

1260 

36 

1800 

5 

1620 

1440 

1800 

9 

45.5 

-45.5 

65.0 

2 

1600 

-20 

2000 

6 

1400 

-1400 

2000 

10 

46.4 

-23.2 

58.0 

3 

-1840 

-69 

2300 

7 

1150 

-920 

2300 

11 

45.0 

30.0 

60.0 

4 

-1575 

70 

1750 

8 

875 

875 

1750 

12 

42.0 

56.0 

70.0 
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Tiiiie(s)  Tiine{s) 

(a)  (b) 

Figure  4.1:  An  example  of  the  pitch  (solid  line)  and  roll  (dashed  line)  behavior  of  the  nominal 
vehicle,  (a)  0(t)  and  (b)  0{t)  and 

The  algorithm  is  now  applied  to  estimate  the  total  angular  velocity,  to  separate  the  rotation 
due  to  climbing  and  pitch  motion,  as  well  as  to  obtain  the  3D  locations  of  four  close  features  with 
respect  to  the  stabilized  coordinate  system. 

4.4.1  High  Signal  to  Noise  Ratio 

We  consider  here  the  situation  in  which  the  vehicle  climbs  uphill  with  a  high  climbing  rate,  ujsy  = 
0.1  rad  •  s"^.  The  performance  of  the  algorithm  is  evaluated  from  50  Monte  Carlo  trials.  Using 
the  image  stabilization  technique  described  in  the  previous  chapter,  the  total  angular  velocity  is 
first  computed  from  eight  listed  distant  points.  The  resulting  x  and  z  components  are  directly 
regarded  as  the  steering  rate  Usx  and  the  roll  velocity  Figures  4.2  and  4.4  respectively  show 
the  corresponding  bias  and  root  mean-square  errors  of  these  estimates.  The  estimate  of  Uy  is 
illustrated  in  Figure  4.3a.  The  test  statistic  6,  defined  in  (4.19),  is  also  calculated  using  the  vertical 
movements  of  points  1-4,  and  is  shown  in  Figure  4.3b.  As  seen  from  Figure  4.3b,  6  exhibits  large 
values  when  maneuvers  occur  (/  =  1.0  s  and  t  =  3.0  s).  Thus  the  start  and  end  of  chmbing 
can  be  detected  instantaneously.  Consequently,  we  take  u^y  into  consideration  from  t  =  1.0  s  to 
t  =  3.0  s.  The  results  of  selective  stabilization  are  shown  in  Figure  4.5.  It  can  be  seen  that  the 
algorithm  separates  both  climbing  and  pitch  phenomena  quite  well.  After  selective  stabilization 
has  been  achieved,  these  estimates  are  used  to  obtain  3D  positions  of  close  features,  i.e.  points 
9-12.  The  initial  guesses  about  these  points  are  listed  in  Table  4.3,  while  the  estimates  are  shown 
in  Figure  4.6.  The  respective  estimates  exhibit  similar  behavior;  therefore  only  the  results  for  the 
12*’^  point  are  displayed.  It  is  observed  that,  when  the  signal  to  noise  ratio  is  high,  the  algorithm 
provides  satisfactory  estimates  which  should  be  useful  for  off-road  vehicular  navigation. 

4.4.2  Low  Signal  to  Noise  Ratio 

We  now  present  some  results  corresponding  to  the  low  signal  to  noise  ratio  case,  i.e.  Ugy  =  0.01  rad¬ 
s'^.  This  corresponds  to  the  scenario  that  the  vehicle  is  moving  uphill  where  the  slope  of  the  hiU 
is  smaller.  Again,  the  performance  is  evaluated  from  50  Monte  Carlo  trials.  Figures  4.7a  and  4.7b 
show  the  estimates  of  the  total  angular  velocity  u)  and  the  test  statistic  6.  Since  the  x  and  z 
components  of  a?  exhibit  similar  behavior  to  that  in  the  high  signal  to  noise  ratio  case,  they  are  not 
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(a)  (b) 

Figure  4.2:  Estimates  of  the  steering  rate  Usx-,  in  the  high  signal  to  noise  ratio  case,  from  50  Monte 
Carlo  trials,  (a)  True  (dashed  line)  and  estimated  (solid  Mne),  (b)  root  mean-square  error. 


(a)  (b) 

Figure  4.3:  Estimate  of  the  climbing-pitch  rate  ojy  and  test  statistic  6,  in  the  high  signal  to  noise 
ratio  case,  from  50  Monte  Carlo  trials,  (a)  Wj,,  (b)  6. 


displayed  here.  In  this  low  signal  to  noise  ratio  case,  contrast  with  Figure  4.3b,  the  test  statistic  b 
does  not  manifest  large  values  at  <  =  1.0  s  and  t  =  3.0  s.  The  start  and  end  of  climbing  are  hard 
to  detect  instantaneously.  However,  by  monitoring  the  vertical  movements  of  the  first  four  distant 
points  at  the  rate  of  2Hz  (the  original  sequence  is  acquired  at  2QHz),  as  shown  in  Figure  4.8, 
the  scheme  observes  that  these  points  move  away  from  their  first  reference  positions  at  t  =  2.5 
s.  The  algorithm  then  reprocesses  the  sequence  from  the  image  acquired  at  t  =  0.5  s.  Similarly, 
after  t  =  3.5  s,  these  points  oscillate  around  new  reference  positions.  The  scheme  declares  that  the 
climbing  ends  at  t  =  3.5  s.  Selective  stabilization  results  based  on  these  conservative  decisions  are 
shown  in  Figure  4.10.  As  seen  from  this  figure,  the  estimate  of  is  noisier  in  this  case.  This 

Table  4.3:  Initial  guesses  of  3D  locations  of  points  9-12. 


Point 

3D  coordinates 

9 

42.3 

-42.9 

60.0 

10 

40.4 

-20.5 

50.0 

11 

37.9 

25.6 

50.0 

12 

36.2 

48.9 

60.0 
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(a)  (b) 

xJO* 


(c)  (d) 

Figure  4.4:  Estimates  of  roll-related  parameters,  in  the  high  signal  to  noise  ratio  case,  from  50 
Monte  Carlo  trials,  (a)  <j>:  true  (dashed  line)  and  estimated  (solid  line)  (b)  (j>:  root  mean-square 
error  (c)  true  (dashed  line)  and  estimated  (solid  line),  (d)  0:  root  mean-square  error. 

leads  to  degradation  in  the  estimates  of  6  and  9,  The  results  corresponding  to  the  3D  locations 
of  close  feature  points  are  shown  in  Figure  4.9.  Compared  to  Figure  4.6,  these  estimates  exhibit  a 
slightly  larger  bias.  This  is  expected  since  the  orientation  of  the  stabilized  coordinate  system  Vs  is 
biased  because  of  the  smooth  rotation  is  mistakenly  introduced  between  t  =  3.0  s  and  t  =  3.5  s. 
Irrespective  of  these  relatively  larger  biases,  the  corresponding  absolute  errors  remain  small.  The 
structure  information  is  therefore  more  or  less  preserved. 

4,5  Conclusions 

This  chapter  has  presented  a  scheme  for  estimating  the  motion  of  a  mobile  robot  as  well  as  the 
relative  positions  of  a  set  of  discrete  features.  The  algorithm  employs  different  dynamic  laws  to 
capture  different  phenomena  such  as  smooth  rotation  as  well  as  pitch  and  roll  motion.  Various 
cues  are  first  utilized  for  the  estimation  of  total  rotation.  The  use  of  an  approximate  kinetic  law 
in  achieving  selective  stabilization  is  then  illustrated.  Schemes  for  the  detection  of  the  beginning 
and  end  of  smooth  rotation  are  investigated.  Due  to  the  separation  of  the  smooth  rotation  and  the 
residual  oscillatory  rotation,  the  3D  locations  of  close  features  are  easily  estimated  with  respect  to 
a  less  perturbed  frame  of  reference.  Finally,  in  addition  to  the  applications  in  image  understanding, 
results  from  the  proposed  selective  stabilization  scheme  provide  useful  information  for  designing  an 
active  suspension  system  which  compensates  for  the  vibration  more  efficiently. 
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Figure  4.5:  Estimates  of  climbing-pitch  parameters,  in  the  high  signal  to  noise  ratio  case,  from  50 
Monte  Carlo  trials,  (a)  true  (dashed  line)  and  estimated  (solid  line)  (b)  uisy'.  root  mean-square 
error,  (c)  9:  true  (dashed  line)  and  estimated  (solid  line)  (d)  9:  root  mean-square  error,  (e)  9:  true 
(dashed  line)  and  estimated  (solid  line),  (f)  9:  root  mean-square  error. 


(a)  (b) 

Figure  4.6:  Bias  and  root  mean-square  error  in  the  estimates  of  3D  locations  of  close  features, 
in  the  high  signal  to  noise  ratio  case,  from  50  Monte  Carlo  trials  (solid  line  corresponds  to  the  x 
coordinate,  dashed  line  to  the  y  coordinate,  and  dashed-dot  line  to  the  2:  coordinate),  (a)  Bias  in 
Point  12,  (b)  root  mean-square  error  in  Point  12. 


(a)  (b) 

Figure  4.7:  Estimate  of  the  climbing- pitch  rate  Uy  and  test  statistic  8,  in  the  low  signal  to  noise 
ratio  case,  from  50  Monte  Carlo  trials,  (a)  u>y^  (b)  8. 
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Figure  4.8:  Vertical  movements  of  distant  points  in  the  low  signal  to  noise  ratio  case.  Points  1  and 
2  are  displayed. 


Figure  4.9:  Bias  and  root  mean-square  error  in  the  estimates  of  the  3D  locations  of  close  features, 
in  the  low  signal  to  noise  ratio  case,  from  50  Monte  Carlo  trials  (solid  line  corresponds  to  the  x 
coordinate,  dashed  line  to  the  y  coordinate,  and  dashed-dot  line  to  the  2:  coordinate),  (a)  Bias  in 
Point  12,  (b)  root  mean-square  error  in  Point  12. 
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(e)  (f) 

Figure  4,10:  Estimates  of  climbing-pitch  parameters,  in  the  low  signal  to  noise  ratio  case,  from  50 
Monte  Carlo  trials,  (a)  tJsy'  true  (dashed  line)  and  estimated  (solid  line)  (b)  ojgy-  root  mean-square 
error,  (c)  0:  true  (dashed  line)  and  estimated  (solid  line)  (d)  0:  root  mean-square  error,  (e)  9:  true 
(dashed  line)  and  estimated  (solid  line),  (f)  9:  root  mean-square  error. 
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Chapter  5 


Feature  Correspondence 


5.1  Introduction 

In  this  chapter,  we  describe  a  localized  feature  point  tracking  scheme.  The  merits  of  both  two- view 
and  multiple-view  based  methods,  mentioned  in  Section  1.1.3,  are  considered  in  designing  such 
an  algorithm  for  finding  trajectories  of  a  set  of  feature  points  over  a  sequence.  Basically,  for  each 
feature  point  of  interest,  a  local  2D  constant  translational  and  rotational  motion  model  is  employed 
to  exploit  the  temporal  information  in  a  sequence.  To  account  for  the  non-smooth  image  motion 
due  to  3D  camera  movements,  a  scheme  is  designed  to  estimate  the  inter-frame  motion  between 
two  windows  in  which  the  point  corresponding  to  the  feature  point  of  interest  are  likely  to  appear. 
Consequently,  for  each  frame,  the  temporal  information  up  to  the  particular  frame  is  explicitly 
stored  in  a  state  vector;  the  state  vector  consists  of  the  position  of  the  corresponding  point  in  the 
particular  frame  as  well  as  the  motion  parameters  between  it  and  the  previous  frame.  Since  the 
state  vector  in  each  frame  contains  the  accumulated  information,  tracking  a  feature  point  over  a 
sequence  is  therefore  decomposed  into  successive  two-view  matching  problems.  However,  unlike 
two- view  based  approaches,  the  resulting  problems  are  not  independent. 

Subsequently,  to  find  the  point  corresponding  to  a  feature  point  in  the  consecutive  frame,  a 
tracking  algorithm  consisting  of  three  stages  is  performed.  The  first  stage  uses  a  Probabilistic 
Data  Association  Filter  (PDAF)  to  obtain  estimates  of  the  2D  motion  parameters  between  the  two 
frames.  The  second  stage  employs  techniques  such  as  image  warping,  correlation  matching,  image 
differential  estimation  and  bilinear  interpolation  to  identify  the  corresponding  point  in  the  frame 
of  interest.  Finally,  the  third  stage  uses  an  EKF  to  update  the  state  vector  so  that  new  temporal 
information  can  be  processed. 

In  addition,  when  a  sequence  of  images  is  considered,  the  scene  in  the  images  changes  constantly. 
Feature  points  detected  in  the  subsequent  frames  are  therefore  likely  to  provide  more  information 
for  other  higher-level  applications  such  as  obstacle  avoidance,  time-to-coHision  estimation,  etc.  A 
scheme  is  thus  designed  to  include  these  newly  extracted  feature  points.  Therefore,  the  algorithm 
has  the  ability  to  track  a  dynamic  set  of  feature  points. 

The  organization  of  this  chapter  is  as  follows.  The  next  section  gives  the  trajectory  model 
employed  in  this  work.  Section  5.3  presents  the  algorithm  for  finding  corresponding  points  between 
two  frames.  A  scheme  for  including  new  feature  points  is  described  in  Section  5.4.  Experimental 
results  are  reported  in  Section  5.5  and  conclusions  are  given  in  Section  5.6. 
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5.2  Trajectory  Model 

There  are,  in  general,  two  ways  to  describe  the  motion  between  two  frames:  3D  and  2D  based 
methods.  3D  based  methods  are  based  on  an  understanding  of  the  camera  motion  while  2D 
based  approaches  apply  2D  transformations.  For  the  3D  based  methods,  since  the  motion  of 
the  camera  needs  to  be  estimated  from  the  images,  finding  corresponding  points  is  coupled  with 
motion/structure  estimation  schemes.  Because  of  the  coupling  of  these  two  stages,  errors  in  one 
stage  are  likely  to  affect  the  accuracy  in  the  other  stage.  Besides,  there  are  applications  in  which  the 
motion  information  is  not  required  while  finding  matching  points  is  important  [9].  It  is  therefore 
desirable  to  have  a  method  independent  of  the  3D  motion  estimation  scheme.  Accordingly,  2D 
based  approaches  are  considered. 

Among  2D  based  techniques,  three  transformations:  affine,  projective  and  polynomial,  are  most 
commonly  employed  in  a  closely  related  problem,  namely,  image  registration  [8,  57].  Their  appro¬ 
priateness  depends  on  the  underlying  motion  of  the  camera.  For  convenience,  let  pjt  :  {x{k),y(k))'^ 
denote  a  point  in  the  k*'^  frame  and  pfc+i  :  {x{k  +  l),y{k  +  1))^  the  resulting  image  point  in  the 
{k  -b  1)®*  frame.  Affine  transformations  account  for  rotation,  translation,  scaling  and  skew  between 
two  images  as  follows: 

Pfc+i  =  AkPk  +  bfe  (5.1) 

where  Ak  and  bfc  are  the  transformation  matrix  and  vector.  Although  affine  transformations  are 
easy  to  use,  perspective  distortions  between  images  are  not  considered.  To  capture  the  image 
motion  more  closely,  projective  transformations  can  be  used.  They  are  expressed  as 

pjk+i  =  {dlpk  +  l)"^(AfcPfc  +  hk)  (5.2) 

where  dk  specifies  the  transform  coefficients  for  correcting  the  perspective  distortion.  Nonetheless, 
there  still  exist  distortions  not  accounted  for  by  affine  and  projective  transformations  [8,  57].  If  even 
more  satisfactory  results  are  desired,  nonlinear  transformations  such  as  polynomial  transformations 
can  be  used.  For  example,  a  second  order  polynomial  transformation  can  be  written  as 

PA:+i  =  CkPkPk^k  +  AkPk  +  bfc  (5.3) 

where  the  matrix  Ck  and  vector  are  the  second  order  coefficients. 

While  the  goal  of  image  registration  is  to  match  two  pictures  globally,  finding  points  corre¬ 
sponding  to  a  feature  point  is,  in  essence,  a  local  problem.  The  locality  results  from  the  perspective 
distortion  as  weU  as  the  camera  motion.  In  this  work,  for  a  feature  point  of  interest,  a  transfor¬ 
mation  is  chosen  to  describe  the  image  motion  between  two  windows  in  which  the  matching  points 
are  likely  to  appear.  Since  only  two  windows  are  concerned,  various  distortions  between  the  two 
windows  are  expected  to  be  small.  An  affine  transformation  is  therefore  used.  Moreover,  to  ex¬ 
ploit  the  temporal  information  of  a  sequence,  constant  translational  and  rotational  dynamics  are 
assumed.  Together,  the  image  motion  of  the  feature  point,  between  the  and  {k  + 1)®*  frames, 
is  modeled  as 

Xj{k  -bl)  =  f[xj(A:)]  -b  Wj{k  +  1)  (5.4) 

with 

Xj(A:)  =  [xj{k)  yj{k)  tj:,{k)  tjy{k)  ej{k)f  (5.5) 
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and 


f[Xj(fc)]  = 


X j{k)  cos  6 j{k)  -  yj{k)  sin  6 j{k)  +  tjx{k)  \ 
Xj{k)  sin^j(A:)  +  yj{k)  cos9j{k)  +  tjy{k) 

tjy{k) 

\  ^i(^)  / 


(5.6) 


where  ixj{k),yj{k))  is  the  position  of  the  corresponding  point  in  the  k*^  frame.  tjx{k),  tjy{k), 
9j(k)  respectively  denote  the  associated  translational  movement  along  the  x,y  directions,  and  the 
rotation  angle,  all  between  the  (k  - 1)®*  image  and  the  k*’^  image;  they  are  referred  to  as  the  motion 
parameters  between  the  (Ar  -  1)®‘  frame  and  the  frame.  Wj(-)  is  zero  mean,  white  noise  which 
is  added  to  account  for  the  deviation  of  the  model  from  the  stated  assumption. 

As  discussed  above,  the  consideration  of  a  locahzed  tracking  scheme  results  in  the  choice  of  the 
simple  affine  transformation  in  this  work.  As  shown  in  the  experiments  later,  this  model  more  or 
less  captures  the  local  image  motion  arising  from  arbitrary  camera  movements.  It  is  therefore  seen 
as  a  transformation  between  the  3D  based  tracking  methods  and  the  2D  global  image  registration 
techniques. 


5.3  Feature  Tracking  between  Two  Frames 

As  mentioned  earlier,  tracking  feature  points  over  a  sequence  is  decomposed  temporally  into  suc¬ 
cessive  two-view  matching  problems.  Therefore,  after  the  trajectory  model  has  been  chosen,  we 
describe  the  tracking  algorithm  between  two  frames  in  this  section.  Without  loss  of  generality,  as¬ 
suming  that  the  feature  point  has  been  tracked  up  to  the  A:***  frame,  the  algorithm  is  described 
in  terms  of  extending  the  trajectory  to  the  {k  + 1)®‘  frame.  For  clarity,  an  overview  of  the  algorithm 
is  first  given  and  each  step  of  the  algorithm  is  then  described  successively. 

5.3.1  Overview  of  the  algorithm 

To  design  an  algorithm  that  combines  the  merits  of  both  multiple- view  and  two- view  based  ap¬ 
proaches,  two  issues  are  considered  in  devising  a  tracking  scheme:  how  to  exploit  the  temporal 
information  such  that  the  search  area  containing  the  corresponding  point  is  small,  and  how  to 
identify  the  corresponding  point  in  the  (A:  -H  frame.  In  our  work,  an  EKF/PDAF  is  employed 
to  process  the  temporal  information  while  a  correlation-matching  based  technique  identifies  the 
corresponding  point  to  sub-pixel  accuracy.  The  resulting  scheme  therefore  involves  three  stages: 
inter-frame  motion  estimation,  corresponding  point  identification  and  temporal  information  filter¬ 
ing. 

More  specifically,  the  two-view  tracking  algorithm  contains  several  steps  summarized  below: 

•  Inter-frame  motion  estimation;  In  the  first  stage,  an  inter-frame  motion  estimation 
scheme  is  applied.  The  scheme  uses  the  accumulated  temporal  information  up  to  the  A:*^ 
frame  to  obtain  an  estimate  of  the  motion  between  two  neighborhoods  which  are  likely  to 
contcdn  corresponding  points  of  the  feature  point. 

•  Corresponding  point  identification:  The  second  stage  identifies  the  corresponding  point 
of  the  feature  point  in  the  (A:  +  1)"‘  frame  using  the  following  procedures: 

1.  Forward  window  warping  and  window  extraction:  Based  on  the  inter-frame  motion 
parameters,  this  step  predicts  the  position  of  the  corresponding  point  in  the  {k  -f  1)®‘ 
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frame.  An  image  warping  technique  is  applied  to  generate  a  window  centered  at  the 
predicted  location  from  a  neighborhood  of  the  fe*^-frame  corresponding  point.  Also, 
another  window  possibly  containing  the  corresponding  point  is  extracted  from  the  {k  + 
1)®*  frame. 

2.  Grid  neighbors  matching;  The  second  step  employs  a  correlation-type  matching  method 
to  find  the  corresponding  point.  However,  the  predicted  location  does  not  necessarily 
faU  onto  a  grid  location  in  the  generated  window.  Alternatively,  matching  points  of  the 
four  nearest  grid  neighbors  are  identified  here. 

3.  Correct  match  verification:  Before  doing  any  further  processing,  if  the  corresponding 
point  is  less  likely  to  be  reliably  identified  based  on  the  matches  from  the  previous  step, 
the  algorithm  stops  tracking  the  feature  point  at  this  step. 

4.  Sub-pixel  accuracy  refinement:  Once  the  algorithm  thinks  the  corresponding  point  can 
be  reliably  found  in  the  {k  +  1)®*  frame,  this  step  applies  an  image-diflferential  based 
scheme  to  improve  the  correlation  matching  results. 

5.  Matching  point  identification:  Finally,  in  the  last  step,  the  algorithm  uses  a  bilinear 
interpolation  scheme  to  obtain  the  corresponding  point  in  the  (k  +  1)®*  frame. 

•  Temporal  information  filtering:  The  third  stage  processes  the  temporal  information  con¬ 
tained  in  the  {k  -f- 1)®‘  frame  by  updating  the  state  vector  Xj(fc  -t- 1). 

After  these  procedures  are  completed,  the  algorithm  can  continue  tracking  the  feature  point 
to  the  (k  +  frame.  We  now  describe  each  step  of  the  two-view  feature  tracking  algorithm  in 
the  following. 

5.3.2  Inter-frame  Motion  Estimation 

Temporal  information  contained  in  a  sequence,  in  general,  can  be  used  to  facilitate  the  feature 
tracking  problem.  To  exploit  the  temporal  information  and  accordingly,  reduce  the  search  area  for 
finding  the  corresponding  point,  an  inter-frame  motion  estimation  scheme  is  devised.  In  this  work, 
the  PDAF  [5]  which  was  originally  proposed  for  tracking  a  moving  object  in  a  cluttered  environment 
is  applied  for  this  step.  A  cluttered  environment,  in  the  context  of  target  tracking,  refers  to  the 
situation  where  there  exist  ambiguities  in  the  observations  such  that  the  designed  tracking  scheme 
does  not  know  which  observation  is  correct,  or  whether  it  is  from  the  tracked  object.  Similarly, 
for  tracking  the  feature  point,  the  ambiguities  appear  whenever  there  is  more  than  one  possible 
matching  candidate  in  the  {k  +  1)®‘  frame. 

For  convenience,  in  addition  to  the  trajectory  model,  define  an  observation  model  which  relates 
the  state  vector  Xj{k  -f  1)  with  the  noisy,  but  correct  observation  Zj{k  -f  1)  as  follows 

Zj{k  -b  1)  =  Hxj(fc  -!-!)  +  nj( 

where  nj(-)  is  the  zero  mean,  white  observation  noise, 

/  1  0  0  0  0  \ 

1^0  1  0  0  0  ) 

Based  on  the  estimates  of  the  state  vector  at  the  frame  xy(/i:|A;), 

±j{k\k)  =  [xj{k\k)  yj{k\k)  i,^{k\k)  ijy{k\k)  0j(k\k)f  (5.9) 
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the  inter-frame  motion  estimation  scheme  first  predicts  the  [k  -f-  l)®*-frame  state  vector,  ij{k  1|A;), 
according  to  (5.4): 

Xj(fc -I- 1|A:)  =  f[xj(A:|fc)]  (5.10) 

In  particular,  the  predicted  location  of  the  corresponding  point,  Zj{k  +  1|A;)  =  {xj{k  +  1|A;),  yj{k  -|- 
l\k))  is  obtained  as 

Xj{k+l\k)  =  cos{ej{k\k))xj{k\k)-sin{ej(k\k))yj{k\k)  +  ijx{k\k) 
yj{k  +  l\k)  =  sm{§j{k\k))xj{k\k)  -f-  cos(^j(A:|A;))yj(^|A:)  +  ijy{k\k) 

Subsequently,  a  window  centered  at  Zj{k  -)-  l|fc)  is  extracted  from  the  (k  +  1)®‘  frame  and  the 
feature  point  extraction  algorithm  reported  in  [36]  is  applied  to  the  window  to  identify  salient  feature 
points.  Due  to  the  lack  of  information  as  well  as  the  desire  for  higher  accuracy,  the  corresponding 
point  is  not  identified  from  the  extracted  points.  Alternatively,  a  validation  gate  based  on  the 
Mahalanobis  distance  [13, 19]  is  constructed  to  select  points  for  further  processing.  More  specifically, 
define  a  validation  gate  centered  at  Zj{k  +  l|fc)  and  with  parameter  7  as  [5,  13]: 

V,- fc+i(7)  =  {z:[z-  zj(k  +  l\k)fSj\k  +  l)[z  -  Zj{k  -h  l|fc)]  <  7}  (5-12) 

where  Sj{k  + 1)  is  the  covariance  matrix  of  the  innovation  vector  z  -  Zj(k  +  IjA:),  and  7  decides  the 
scope  of  the  validation  gate.  A  set  of  extracted  points  is  selected  if  their  Mahalanobis  distances 
are  less  than  7.  Without  loss  of  generality,  it  is  assumed  that  there  are  mj{k  -b  1)  points,  denoted 
as  {zj^i{k  -f  l),i  =  1, . ..,mj{k  -f  1)},  inside  the  validation  gate  V j^k+xin)-  For  clarity.  Figure  5.1 
shows  a  situation  where  five  points  have  been  extracted,  the  three  points  inside  the  validation  gate 
will  be  selected. 


Figure  5.1:  Illustrations  of  the  PDAF:  z  is  the  predicted  location  and  the  ellipse  represents  the 
associated  validation  gate.  {zj,i  =  1,...,5}  are  the  five  extracted  feature  points.  Among  them, 
{Z3,Z4,Z5}  are  inside  the  validation  gate.  The  position  marked  by  x  corresponds  to  the  position 
estimates  computed  by  the  PDAF. 

Among  the  mj{k  +  1)  extracted  points,  due  to  changes  between  images,  each  of  the  mj{k  +  1) 
points  can  be  the  corresponding  point.  In  other  words,  if  we  consider  these  points  as  noisy  corre¬ 
sponding  points,  we  face  a  situation  with  ambiguities  in  observations.  To  process  the  information 
contained  in  these  noisy  observations,  the  PDAF  is  thereafter  employed. 
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In  essence,  the  PDAF  obtains  the  information  by  associating  diiferent  weights  to  different 
observations.  A  weight  is  assigned  to  an  observation  according  to  the  a  posteriori  probability  of  an 
event  which  states  that  the  corresponding  observation  is  the  correct  one  given  the  past  data.  As 
shown  in  [5],  under  the  assumption  that  the  observations  are  Normally  distributed  about 
these  weights  can  be  easily  obtained.  For  convenience,  denote  these  weights  by  +  1),  i  = 

0,1,...,  mj{k  + 1)}  respectively.  Note  that  /3j,o(^  + 1)  is  the  weight  assigned  to  the  event  that  none 
of  the  observations  are  correct. 

After  each  weight  has  been  computed,  the  PDAF  (or  the  inter-frame  motion  estimation  scheme) 
processes  the  information  from  the  mj(k  -|- 1)  observations  as  follows 


Xj{k  -b  l\k  -b  1)  =  -b  1|A;)  -b  -b  1) 


■mj(fc+l) 

i=i 


(5.13) 


where  Kj(A:  -b  1)  is  the  gain  matrix  defined  similarly  as  the  EKF, 

Vj^i{k  -b  1)  =  Zj^i{k  -b  1)  -  Zj{k  +  1|A:)  (5.14) 

For  illustration  purpose,  the  first  two  components  of  the  state  vector  Xj(A:-bl|^-bl),  (5j(A;-blifc  +  l), 
yj{k  -b  1|A;  -b  1)),  are  supposedly  marked  by  x  in  Figure  5.1.  It  is  worth  noting  that  if  additional 
information  is  available  such  that  the  ambiguities  in  observations  are  resolved,  then  a  PDAF  is 
simplified  to  an  EKF.  This  justifies  the  state  vector  prediction  in  (5.10). 

Consequently,  from  (5.13),  the  estimates  of  motion  parameters,  i.e.  the  other  three  elements  in 
Xj{k  -b  l\k  +  1):  ijx(k  +  1|^  +  l),ijy{k  +  l\k  -b  l),6j{k  -b  1|A;  -b  1),  are  obtained.  We  refer  to  them 
as  the  inter-frame  motion  parameters.  Since  the  information  in  the  {k  -b  1)®*  frame  is  incorporated, 
the  inter-frame  motion  parameters  more  or  less  capture  the  image  motion  of  the  neighborhoods  of 
the  feature  point  due  to  the  3D  movement  of  the  camera.  With  these  parameters  available,  the 
algorithm  proceeds  to  the  next  stage  to  identify  the  corresponding  point. 


5.3.3  Corresponding  Point  Identification 

In  this  stage,  the  corresponding  point  of  the  feature  point  in  the  {k  -b  1)®*^  frame  is  found  using 
a  correlation  matching  approach  followed  by  an  interpolation  scheme.  The  correlation  matching 
method  employed  is  similar  to  the  block  matching  technique;  however,  inter-frame  motion  parame¬ 
ters  are  used  so  that  the  search  area  for  the  corresponding  point  is  small  and  the  matching  results 
are  more  accurate.  On  the  other  hand,  the  interpolation  scheme  handles  the  problem  due  to  the 
corresponding  point  not  being  at  a  grid  location.  This  is  important  for  tracking  a  feature  point 
over  a  long  sequence.  Approximating  the  corresponding  point  by  its  nearest  grid  neighbor  often 
leads  to  the  situation  that  the  corresponding  point  slowly  moves  away  from  the  right  position.  The 
procedures  used  in  this  stage  are  presented  in  the  following. 


A.  Forward  Window  Warping  and  Window  Extraction  The  first  step  in  identifying  the 
corresponding  point  consists  of  obtaining  two  windows  from  the  and  (fc-bl)®*^  frames  respectively. 
We  refer  to  the  window  obtained  from  the  k^^  frame  as  the  reference  window  Ij^r-  The  other  one  is 
called  the  target  window 

Consider  first.  Given  the  A;***  frame  and  inter-frame  motion  parameters,  we  want  to  predict 
the  reference  window  Ij^r-  Similar  problems  have  been  studied  in  the  area  of  image  warping  [57]. 
Basically,  Ij^r  is  an  image  that  the  algorithm  believes  the  neighborhood  of  the  corresponding  point 
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in  the  {k  +  1)®‘  frame  should  look  like.  To  obtain  a  more  accurate  prediction  regarding  the 
position  of  the  corresponding  point,  Zjj{k  +  l|fc)  =  (xj^rik  +  is  used: 

Xj,rik  +  1|A:)  =  cos((9j(k  +  l\k  +  l))xj{k)  -  sm{0j{k  +  l|k  +  l))yj{k) 

-hijxik  +  1|A;  +  1) 

yj^rik+l\k)  =  sm{6j{k  +  l\k+l))xj{k)  +  cos(ej(k  +  l\k  +  l))yj(k) 

-\-tjy{k  +  \\k  +  1) 

where  ij{k)  =  {xj{k),  yj{k))  is  the  position  of  the  previously  found  frame  corresponding  point. 
Note  that  {xj{k),  yj{k))  is  used  instead  of  {xj(k\k),  yj{k\k)).  Subsequently,  assume  that  pixels  which 
are  close  to  lxj{k),  yj{k))  in  the  k^^  frame  undergo  the  same  inter-frame  motion,  their  locations  in 
the  {k  -h  1)®*  frame  are  predicted  in  a  similar  way.  After  that,  the  predicted  pixels  are  assigned  with 
the  same  intensity  values  as  the  original  pixels.  This  yields  the  reference  window  whose  center 
is  Zj,T{k  +  l|k). 

On  the  other  hand,  the  target  window  Ij^t  is  easier  to  obtain.  By  centering  at  +  1|^),  Ij,t 
is  directly  extracted  from  the  {k  -t- 1)®‘  frame.  To  avoid  potential  confusion,  we  denote  the  center 
of  as  Zj^t{k  -1- 1|^;)  =  (xj^tik  H-  l\k),yj,tik  -K  1|A;))  ,  although  Zj^t{k  +  1|A:)  has  the  same  value  as 
Zj,r{k  -f-  1|^)- 

Since  is  obtained  with  the  knowledge  of  the  inter-frame  motion,  a  correlation  matching 
method  can  be  employed  to  find  the  corresponding  point.  Thus,  given  /j,r  and  if  Zj^r{k  +  1|A:) 
happens  to  be  a  grid  point,  then  the  corresponding  point  of  the  feature  point  can  be  directly 
found.  However  Zj^r{k  +  1|^),  in  general,  is  not  located  at  a  grid  point.  We  describe  how  to  handle 
this  situation  next. 


B.  Grid  Neighbors  Matching  Correlation  matching  techniques  typically  match  a  grid  point  in 
one  image  into  a  grid  point  in  another  image.  In  our  case,  the  problem  appears  when  the  predicted 
location  +  1|A:)  does  not  coincide  with  a  grid  point.  A  scheme  suggested  in  [61]  is  employed 
to  solve  this  problem.  The  approach  is  basically  composed  of  two  steps:  (1).  Find  matching  points, 
in  /j,t,  of  the  four  nearest  grid  points  of  Zj>(k  -f  l|fc)  using  correlation  matching  methods.  (2). 
Interpolate  the  results  to  obtain  the  desired  corresponding  point.  We  describe  the  first  step  here. 
The  second  step  is  discussed  later. 

For  convenience,  denote  the  four  nearest  grid  points  ofzj^r{k+l\k)  by  {zj^i{k+l\k),i  =  1, . .  .,4}, 


1|^) 

Zj,2{k+l\k) 
Zj,3{k+  1|A;) 
Zj,4{k+l\k) 


+  1|^)],  [hr{k  +  1|A:)]) 

l[xj,r{k  -I-  ijA:)],  [yj^rik  +  ij^)]  +  1) 

([x,,r{k  +  l\k)]  +  l,  [M^+1|A:)]) 

([x,>(fc  +  l|k)]-M,  [M^+l|k)]  +  l) 


(5.16) 


where  [•]  represents  the  floor  function  which  rounds  a  real  number  to  the  nearest  smaller  integer. 
Without  loss  of  generality,  we  focus  on  finding  the  matching  point  of  Zj^i(k  +  Ilk)  here.  The 
matching  points  of  the  other  three  grid  points  can  be  found  similarly. 

To  employ  the  correlation  matching  method,  a  template  Q,i.  ^{zj^i{k  +  l\k))  centered  at  Zj,i(k4- 
l|k)  is  first  created  from  Ij,r-  Then,  in  -f-  l|k)),  to  put  more  emphasis  on  pixels  close  to 

+  l|k),  each  pixel  is  further  assigned  with  a  different  weight  according  to  the  following: 


8max(|/|,|m|) 
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— 


if(/,  m)  =  (0,0) 
otherwise 


(5.17) 


where  c  is  a  constant  and  (/,  m)  are  integers . such  that  +  1|^)]  +  +  1|^)]  +  m)  G 

+  1|A^)).  The  resulting  template  is  thereupon  seen  as  a  weighted  template.  Note  that 
for  any  q  G  a  template  ^(q)  centered  at  q  with  similar  weight  assignments  can  also  be 
obtained. 

Subsequently,  if  we  employ  the  weighted  templates  and  define  the  similarity  measure  between 
P  =  iPi,P2)  e  Ij,r  and  q  =  (91,5-2)  £  Ij,t  as 


C(p,q) 

iVi(p).iV2(q) 


(5.18) 


where  fl-i(-)?P2(0  are  the  intensity  functions  of  the  pixels  in  Ij^r  and  Ij^t  respectively,  and 


C'(p,q) 

iVi(p) 

iV2(q) 

5i(p) 
92  (q) 


XI  Wm[9i(pi  +  l,P2  +  m)-  9i(p)]  •  [92(91  +  /,92  +  m)  -  92(q)] 
l^m 


'^PlmigiiPl  +  l,P2  +  m)-  9i(p)] 

l,m 


'^pim[g2{qi  +  l,q2  +  m)-  92(q)]^ 


/,m 


j^Y.SiiPi  +  hP2  +  m) 


N 


l,m 


'^g2iqi  +  hq2  +  m) 


l,m 


(5.19) 

(5.20) 

(5.21) 

(5.22) 

(5.23) 


with  N  being  the  number  of  pixels  in  both  fi/^  ^(p)  and  Jl/j  ,(q),  then  the  matching  point  of 
Zj^i{k  +  1|A)  is  obtained  by  searching  for  a  grid  point  in  which  has  the  highest  similarity 
measure  with  Zj,i{k  +  1|^).  (For  example,  — 5  <  /,  m  <  5  and  c  =  2  are  used  in  our  experiments.) 

A  few  remarks  regarding  the  above  correlation  matching  scheme  are  given  in  the  following. 
First,  the  similarity  measure  (5.18)  has  the  same  property  as  the  weU-known  correlation  coefficient 
[61]: 

I^W,.I<1  (5.24) 

Second,  the  use  of  a  weighted  template  is  expected  to  achieve  better  localization  because  of  the 
imperfect  feature  detection  scheme.  By  this,  we  mean  that  the  extracted  feature  points  usually 
are  not  located  exactly  at  the  corners  due  to  quantization  effects.  Typically,  they  are  a  few  pixels 
away  from  the  boundaries.  Experiments  show  that  giving  higher  weights  to  pixels  near  the  center 
of  the  template  helps  in  achieving  better  localization  later.  Third,  since  the  inter-frame  motion 
has  been  considered,  to  find  the  matching  grid  point  of  Zj^i{k  +  IjA:),  only  a  small  area  centered 
at  +  l\k)],  [yj,t{k  +  1|^)]  -f- 1)  in  Ij,t  needs  to  be  searched.  (In  particular,  a  10  X  10  window 

is  searched  in  the  experiments.)  Figure  5.2  illustrates  a  possible  configuration  obtained  in  this 
step.  For  convenience,  we  denote  the  resulting  matching  grid  points  of  {zj^i(k  +  1),  i  =  1, ... ,4}  by 
{yj,i(^  +  !)>*=  i,...,4}. 

Up  to  now,  we  have  implicitly  assumed  that  the  point  corresponding  to  the  9*^  feature  point 
exists  in  the  {k  4- 1)®*  frame,  and  the  neighborhoods  of  corresponding  points  in  the  k*'^  and  {k  +  iy^ 
frames  are  similar.  However,  there  exist  situations  where  these  assumptions  may  not  be  valid.  We 
next  discuss  a  scheme  which  verifies  these  assumptions. 
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Figure  5.2;  Illustrations  of  grid  neighbors  matching:  Zj,r,Zj^t  are  marked  by  x  in  and  Ij^t 
respectively.  The  dashed  square  in  represents  the  template  The  dashed  square  in 

Jj,t  is  the  search  area  where  the  matching  point  of  Zj^i,  yj^i,  is  to  be  searched  for.  The  matching 
points  of  all  four  grid  neighbors  of  are  marked  by  +  in  Ij^f 


C.  Correct  Match  Verification  It  is  well  known  that  when  a  feature  point  is  tracked  to  the 
(k  +  1)®*  frame,  the  corresponding  point  may  be  occluded.  Even  if  the  corresponding  point  is 
not  occluded,  the  relative  structures  in  the  neighborhoods  of  corresponding  points  in  the  k^^  and 
(k  +  1)®*  frames  may  change  dramatically.  Both  situations  lead  to  incorrect  matches  using  the 
previously  described  correlation  matching  method.  Therefore,  before  advancing  to  the  next  step, 
the  hypothesis  that  the  matches  {(zj^k  +  l|A:),yj,i(A:  +  1)), ?  =  1, . .  .,4}  are  correct  is  tested  here. 

We  select  a  threshold,  say  Tff,  as  follows,  to  decide  whether  (zj,,(fc  +  'l\k),yj^i{k  +  1))  is  a 
correct  match  for  each  i  6  {1, . .  .,4}: 

I  +  1))  >  +  1))  is  correct 

I  +  l|^).yi,i(^  +  1))  <  TH  otherwise 

For  convenience,  denote  the  number  of  correct  matches  among  {izj,i{k  +  l\k),yj^i{k  +  l)),i  = 
1, ...  ,4}  by  n.  Three  cases  are  considered  in  the  following: 

Case  1:  n  <  2 

In  this  case,  more  than  two  matches  of  grid  neighbors  of  +  1|^)  S'C®  likely  to  be  incorrect. 
Consequently,  the  algorithm  feels  that  the  real  point  corresponding  to  the  feature  point  cannot 
be  rehably  found  in  the  {k  +  1)®*  frame.  No  further  tracking  will  be  attempted. 

Case  2:  n  =  3 

In  this  case,  one  of  the  matches  is  regarded  as  unreliable.  Without  loss  of  generality,  we  assume 
that  yj,i{k  +  1)  is  the  unreliable  matching  point.  Since  there  are  three  other  correct  matches, 
the  algorithm  tries  to  correct  the  wrong  match  using  an  extrapolation  scheme.  More  specifically, 
because  {zj^i(k  +  1|J!:),  i=  1, . .  .,4}  form  a  square  in  the  extrapolation  scheme  assumes  that 
{yj,i(^  +  1),  f  =  1, . .  .,4}  should  form  a  parallelogram  in  Ij,f  The  matching  point  of  Zj,i{k  +  1|A;) 
is  then  replaced  by 

yj^i{k  +  1)  =  yj,2{k  +  1)  +  yj,3ik  +  1)  -  yj^k  +  1)  (5.26) 

For  clarity,  we  illustrate  this  procedure  in  Figure  5.3.  Assume  that  yj,i{k  +  1)  is  originally 
matched  to  the  conceivably  wrong  point  marked  by  +  due  to  changes  in  the  neighborhood.  Applying 
(5.26)  yields  another  matching  point  for  Zj,i(A:  +  l\k)  as  indicated  in  the  figure. 
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Figure  5.3:  Illustrations  of  case  2  of  correct  match  verification:  x  denotes  the  real  corresponding 
point.  The  original  yj,i  is  denoted  by  +  while  the  extrapolated  one  is  denoted  by  •. 


Case  3:  n  =  4 

In  this  case,  {{zj,i{k  +  l\k),yj^i{k  +  1)),  i  =  1, . . . ,  4}  are  aU  confirmed  to  be  correct. 

Among  three  cases,  if  either  case  2  or  case  3  applies,  the  algorithm  proceeds  to  the  next  step. 


D.  Sub-pixel  Accuracy  Refinement  Since  {(zj_i(fc-l-  l|fc), i  =  1,...,4}  are  only  matched  to 
grid-level  accuracy  by  the  correlation  matching  scheme,  this  step  tries  to  improve  the  matching 
accuracy  to  the  sub-pixel  level.  In  our  work,  an  image-differential  based  technique  [27]  is  employed 
for  the  purpose  [61].  Again,  we  illustrate  this  scheme  in  terms  of  {zj,i{k  -b  l|/:),yj4(A:  -f- 1)). 

Recall  that  g2{-)  is  the  intensity  function  of  pixels  in  /j,*.  Under  the  assumption  that  g^i-)  has 
an  offset  A  =  {Sx,6y)  relative  to  another  intensity  function  g^i-)  a.t  yj,i(k  +  1): 

52(yj,i(^  +  1))  =  92iyj,i{k  +  1)  +  A)  (5.27) 


the  image  differential  scheme  approximates  the  difference  between  ?2(yy,i(^+l))  and  52(yi,i(^+l))5 
d(yj^i{k  +  1)),  using  the  first-order  Taylor  series  expansion: 

+  1))  =  ffaCyi.iCA:  +  1))  -  52(yi,i(^  +  1))  (5-28) 

~  <Vg2{yUk  +  l)),A>  (5.29) 


where  <  •,  •  >  represents  the  inner  product  of  the  arguments,  and  V52(yi,i(^  +  1))  is  the  gradient 
vector, 

o.  I  _  fdg2iyj,i{k  +  1))  dg2iyj,i{k  +  l)y  ^ 

^S2iyj,i{k  +  1))  =  [ - - 


(5.30) 


Subsequently,  assume  that  the  approximation  (5.29)  holds  for  a  small  neighborhood  around 
yj,i{k  -f  1)  of  size  {2u>d  -b  1)  x  {2ojd  +  1);  then  a  set  of  equations  can  be  found: 


GA  =  D 


(5.31) 
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where 


G  =  [V52(yi,x(^  +  1)  +  ),---,V52(yj,i(fc  +  i)  +  'iio,o), 

...,Vp2(yi,i(fe  +  i)  + wJF  (5-32) 

D  =  Kyi,i(^  +  1)  +  . . . ,  d(yi,i(A;  +  1)  +  mo,o), 

•  •  • )  ^(yj,i(^  +  1)  +  (5.33) 


with 

=  {‘^11^2)'^  1  -‘^d  <  t/i,U2  <  i^d  (5.34) 

such  that  (yj,i(^  +  1)  +  “*^1,1^2)  is  in  the  {2ujd  +  1)  x  (2wd  +  1)  neighborhood  of  yj,i{k  +  1). 

Afterwards,  the  offset  vector  A  is  obtained  from  (5.31)  using  a  least  square  approach.  (In  our 
experiments,  Ui  =  3.)  %,i(^  +  1|A;)  is  then  matched  to  yj,i(^  +  1)  =  (^j,i(^  +  l))2/i,i(^  +  1))  = 
(yi,i(^  +  1)  +  A).  Similarly,  the  sub-pixel  matching  of  the  other  three  grid  points  can  also  be 
obtained.  We  denote  these  four  sub-pixel  matching  points  by  {y'j^i{k  -f  l),i  =  1,. .  .,4}. 


E.  Matching  Point  Identification  After  {yj^i{k  +  l),f  =  1,...,4}  have  been  obtained,  the 
algorithm  identifies  the  corresponding  point  of  the  feature  point  using  a  bilinear  interpolation 
scheme  in  this  step. 

The  scheme  uses  matches  {(zj^i{k  -t-  l\k),yji{k  -t-  1)),*  =  1,...,4}  to  find  the  corresponding 
point  of  the  feature  point.  More  specifically,  assume  that  for  any  pixel  p  =  (pi,P2)  €  Ij,r  which 
is  inside  or  on  the  boundary  of  the  square  formed  by  {zj^i{k  -1-  1|A:),  i  =  1, . . . ,  4},  its  matching  point 
in  /j,i,  q  =  {qi,q2),  can  be  obtained  as 


qi  =  0:1  -I-  a2Pi  +  013P2  +  Q!4PiP2 
92  =  Pi  +  P2PI  +  PzP2  +  P4P1P2 


(5.35) 


where  {ai,Pi,  i  =  1, . .  .,4}  are  constant  coefficients  to  be  found. 

Then,  since  the  matching  points  of  {zj^i{k  -1-  Ijfc),?  =  1,. .  .,4}  have  been  obtained,  {ai,/3i,i  = 
1,...,4}  can  be  found  easily  [57,  61].  Accordingly,  using  these  coefficients  and  (5.35),  the  corre¬ 
sponding  point  of  the  feature  point  Zj(k  -f  1)  =  {xj{k  -f  1),  yj{k  4- 1)),  are  computed  by 

r  zj{k  +  i)  =  y;-,i(A;  4-1) +  c:r[yi,3(^  +  i)-yi,i(^  +  i)]  +  %[yi,2(^  +  i)-yj, 1(^  +  1)] 

j  4-Ca;Cj,[yj,i(^  4- 1)  4- yj,4(A  4-  1)  —  yj,2(^  +  1)  ~  yi,3(^  +  1)]  (5.36) 

I  Cx  =  ®i,r(^+  1|^)  -  [^J>(^  + 1|*)] 

[  Cy  =  yj,rik  +  l\k)  -  [yj,rik  +  l\k)] 

where  {xj,r{k  +  l\k),yj,r{k  +  l|fc))  and  {[xj,rik  +  l\k)],[yjA^  +  1|^)])  l^^ve  been  defined  in  (5.15) 
and  (5.16)  respectively. 

This  ends  the  stage  of  corresponding  point  identification.  The  algorithm  then  advances  to  the 
next  stage  of  temporal  information  filtering. 


5.3.4  Temporal  Information  Filtering 

To  facilitate  the  tracking  scheme,  the  temporal  information  between  the  k^^  and  (fc  4-  1)®‘  frames 
is  processed.  As  mentioned  earlier,  accumulated  temporal  information  up  to  the  (k  +  1)®‘  frame 
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is  stored  in  Xj{k  +  1)  in  our  work.  For  convenience,  we  repeat  the  trajectory  model  (5.4)  and  the 
observation  model  (5.7)  here: 


Xj{k+1)  =  f[xj(fc)]  +  Wj(fc  +  1) 
Zj{k  +1)  =  lixj{k  +  1)  +  nj{k  +  1). 


Then,  given  the  corresponding  point  Zj{k  +  1),  the  current  stage  employs  an  EKF  to  obtain  the 
estimate  of  Xj(k  +  1),  Xj(A;  +  l|fc  +  1). 

Since  f  is  nonlinear,  in  order  to  apply  the  EKF,  the  following  matrix  is  first  defined: 


F  = 


dx' 


The  temporal  information  is  then  processed  by  performing  following  steps  successively: 


(5.37) 


•  Step  1:  State  and  covariance  propagation 

Xjik  +  l\k)  =  f[xj{k\k)]  ^ 

Pj(k  +  IjA;)  =  F[xj(A:|fc)]Pj(^|fc)F[xj(A:|A:)]^  +  Qj(fc  +  1)  v  •  1 

where  x.j{k\k)  is  defined  in  (5.9).  Pj{k\k),Pj(k  +  l|A:),Qj(A:  +  1)  are  covariance  matrices  of 
Xj{k\k),Xj{k  +  1|A:)  and  Wj(fc  +  1)  respectively. 

•  Step  2:  State  and  covariance  update 

Kj{k  +  1)  =  Pj{k  +  l\k)B.'^[Hpj{k  +  l\k)Ii^ +  Kj{k+1)]-^ 

Xj(fc  +  1|A:  +  1)  =  Xj(fc  +  l|fc)+Kj(A:  +  l)[zj(fc  +  l)-Hxj(;:  +  l|A))]  (5.39) 

p/(A:  +  l|A:  +  l)  =  [I  -  Kj(;fc  +  l)H]Pj(ifc  +  l|ib) 

where  Rj(A;  + 1)  is  the  covariance  matrix  of  nj(fc+ 1),  and  I  is  the  identity  matrix.  Kj(/;  + 1) 
represents  the  gain  matrix,  while  Xj(A:  +  l|fc+l)  is  the  updated  state  vector  and  Pj(A:+l|A:  +  l) 
is  the  associated  covariance  matrix. 


Note  that  in  applying  the  EKF  to  process  the  temporal  information,  the  corresponding  point 
Zj(k  +  1)  is  assumed  to  be  normally  distributed  around  Hxj(fe  +  1|A;).  This  is  a  convenient  assump¬ 
tion,  and  it  also  works  well  in  our  experiments.  Moreover,  Xj{k  -f  1|A:  + 1),  in  particular  the  position 
estimates  {xj{k  +  1|^  +  l),yj{k  -f  1|A:  -f- 1)),  are  only  used  to  exploit  the  temporal  information  as 
shown  in  (5.11).  The  observation  Zj{k  +  1)  should  be  used  as  the  corresponding  point  of  the  j*'^ 
feature  point  in  the  {k  +  1)®‘  frame  whenever  higher-level  applications  are  considered. 

After  the  temporal  information  is  processed,  the  task  of  tracking  the  feature  point  to  the 
{k  -f  1)®*  frame  is  completed.  The  algorithm  can  now  continue  tracking  the  feature  point  to  the 
(k  +  2)“*^  frame.  Before  presenting  the  experimental  results  of  the  tracking  algorithm,  we  digress 
briefly  to  present  a  scheme  for  including  new  feature  points  detected  in  the  (fc  -|- 1)®‘  frame  in  the 
next  section. 


5.4  Inclusion  of  New  Features 

When  tracking  a  set  of  feature  points  over  a  sequence,  it  is  likely  that  some  points  disappear 
after  some  frames.  Accordingly,  related  tasks  may  be  affected  since  the  amount  of  information  is 
reduced.  (Here,  each  tracked  feature  point  is  assumed  to  provide  useful  information,  although  in 
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some  applications,  only  a  minimum  number  of  point  correspondences  is  required.)  In  addition,  since 
the  scene  in  the  sequence  constantly  changes,  it  is  desirable  to  track  feature  points  in  the  successive 
frames  which  may  well  contain  more  information  than  feature  points  currently  being  tracked.  We 
suggest  a  scheme  which  considers  the  problem  of  including  new  feature  points  detected  later  in  the 
sequence. 

Assume  that  the  algorithm  has  already  completed  tracking  all  feature  points  from  the  frame 
to  the  {k  +  1)®‘  frame.  Without  loss  of  generality,  suppose  that  there  remain  M  feature  points 
in  the  {k  +  frame  and  consequently,  there  exist  M  validation  gates,  {Vi,jt+i(7),  V2,a:+i(7)) 
. . . ,  Vivf,A:+i(7)}-  Note  that  the  M  validation  gates  are  defined  analogously  to  (5.12).  Then  since 
Vj,fc4.i(7)  represents  a  neighborhood  of  the  corresponding  point  of  the  y***  feature  point,  we  assume 
that  points  inside  Vj,fc+i(7)  more  or  less  carry  the  same  information  as  the  feature  point  for 
other  applications.  Equivalently,  for  points  extracted  from  the  {k  +  1)®*  frame  by  the  algorithm 
reported  in  [36],  only  those  points  outside  all  of  the  M  validation  gates  are  regarded  as  new  feature 
points.  The  tracking  algorithm  therefore  starts  tracking  these  points  only. 

For  example,  consider  Figure  5.4.  Assume  that  the  tracking  algorithm  maintains  eight  trajecto¬ 
ries  in  the  (fb-f  1)®*  frame.  This  results  in  eight  validation  gates.  Meanwhile,  the  feature  extraction 
algorithm  extracts  nine  feature  points  from  the  (Ar4- 1)®*^  frame.  Since  only  and  Zg  are  outside 
aU  the  eight  validation  gates,  the  algorithm  only  recognizes  these  three  as  new  feature  points. 


Figure  5.4:  Illustrations  of  the  scheme  of  including  new  feature  points:  The  tracking  algorithm 
tracks  eight  feature  points  to  the  {k  -t-  1)®*  frame  and  forms  eight  validation  gates.  Zj, . . . ,  Zg  are 
nine  newly  extracted  feature  points.  Only  z'ljZg  and  z^  are  considered  as  new  feature  points  in  the 
{k  4- 1)®‘  frame. 

As  shown  in  the  experiments  later,  the  proposed  scheme  is  quite  efficient.  It  not  only  handles 
the  problem  of  the  decreasing  number  of  tracked  feature  points,  it  also  prevents  the  number  of 
feature  points  from  growing  too  fast.  This  is  because  that  when  more  feature  points  are  tracked, 
the  image  region  covered  by  the  associated  validation  gates  also  enlarges. 

This  completes  the  description  of  the  scheme  for  including  new  feature  points  as  well  as  our 
algorithm  for  tracking  a  dynamic  set  of  feature  points.  The  next  section  presents  experimental 
results  on  four  real  image  sequences. 
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5.5  Experimental  Results 

In  this  section,  tracking  results  are  presented  for  four  real  image  sequences  taken  by  cameras 
undergoing  different  types  of  motion.  A  tracking  list  which  contains  the  corresponding  points  as 
well  as  the  new  feature  points  is  created  and  updated  at  every  frame.  For  visual  purposes,  only  the 
trajectories  of  the  feature  points  tracked  from  the  first  frame  as  well  as  the  new  feature  points  added 
to  the  tracking  list  at  subsequent  frames  are  displayed.  The  dynamic  behavior  of  the  algorithm  is 
shown  in  a  table  which  lists  the  number  of  feature  point  trajectories  being  maintained  or  removed 
from  the  tracking  list  and  the  number  of  new  points  selected  from  every  frame. 

5.5.1  UMASS  PUMA2  Sequence 

The  first  sequence  is  known  as  the  UMASS  PUMA2  sequence;  it  consists  of  thirty  256  x  256  frames. 
The  camera  is  connected  to  the  end  of  a  PUMA  robot  arm  and  rotates  about  a  rotation  center 
which  is  close  to  the  image  center.  Figure  5.5  shows  the  trajectories  for  a  set  of  feature  points 
automatically  extracted  from  the  first  frame  by  the  algorithm  reported  in  [36];  the  trajectories  are 
shown  up  to  the  19^^  and  30*^  frames.  The  new  feature  points  extracted  by  the  feature  extraction 
algorithm  from  frames  3,  19  and  30  are  also  shown  in  Figure  5.5  in  addition  to  the  labeled  points 
which  were  added  to  the  tracking  list  at  different  time  instants.  The  number  of  feature  points  being 
tracked  varies  with  time,  as  shown  in  Table  5.1.  As  seen  in  Table  5.1,  the  algorithm  for  adding  new 
points  to  the  tracking  list  efficiently  maintains  the  number  of  points  on  the  list. 

Table  5.1:  The  number  of  feature  points  in  the  tracking  list  for  the  UMASS  PUMA2  Sequence 


frame  number 

1 

2 

3 

4 

5 

6 

7 

8 

9 

10 

11 

12 

13 

14 

15 

tracked  points 

0 

21 

19 

28 

31 

35 

38 

40 

41 

40 

41 

43 

43 

44 

46 

extracted  points 

23 

23 

22 

26 

24 

27 

29 

29 

28 

28 

25 

16 

19 

20 

24 

new  points 

23 

0 

9 

4 

5 

5 

3 

3 

4 

4 

3 

1 

2 

2 

5 

frame  number 

16 

17 

18 

19 

20 

21 

22 

23 

24 

25 

26 

27 

28 

29 

30 

tracked  points 

49 

48 

49 

48 

50 

49 

51 

50 

53 

50 

52 

52 

53 

54 

53 

extracted  points 

18 

21 

20 

21 

20 

21 

20 

19 

23 

25 

24 

24 

27 

24 

15 

new  points 

2 

3 

1 

3 

1 

3 

0 

4 

0 

3 

0 

3 

2 

1 

1 

5.5.2  Coke  Can  Sequence 

The  second  sequence  is  the  Coke  Can  Sequence,  in  which  the  camera  is  approaching  the  scene,  with 
the  Focus  of  Expansion(FOE)  located  on  the  coke  can.  Fifteen  frames  chosen  from  the  densely 
sampled  sequence  (every  tenth  frame)  are  used.  The  original  512  x  512  images  are  down-sampled 
to  256  X  256  before  applying  the  algorithm.  The  resulting  trajectories  from  the  first  frame  to 
the  lO*’^  and  frames  are  shown  in  Figure  5.6.  As  seen  from  the  figures,  because  of  the  pure 
translation  of  the  camera,  the  trajectories  of  the  feature  points  diverge  from  the  FOE.  The  number 
of  tracked  feature  points  at  each  time  instant  is  listed  in  Table  5.2.  The  new  feature  points  added 
at  the  2'^'^,  10*^^,  and  15‘^  frames  are  also  marked  in  Figure  5.6. 
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Table  5.2:  The  number  of  feature  points  in  the  tracking  list  for  the  Coke  Can  Sequence 


frame  number 

1 

2 

3 

4 

5 

6 

7 

8 

9 

10 

11 

12 

13 

14 

15 

tracked  points 

0 

12 

13 

16 

18 

20 

22 

22 

22 

26 

27 

28 

28 

28 

28 

extracted  points 

13 

12 

15 

12 

13 

17 

15 

15 

16 

15 

14 

17 

15 

17 

14 

new  points 

13 

1 

4 

2 

2 

2 

1 

1 

5 

1 

1 

2 

0 

1 

1 

5.5.3  Rocket  ALV  Sequence 

The  third  sequence  is  the  30-frame  UMASS  Rocket  ALV  Sequence.  Again,  the  512  x  512  images  are 
down-sampled  to  256  x  256  before  applying  the  algorithm.  In  this  sequence,  the  camera  is  mounted 
on  a  vehicle  which  appears  to  be  moving  along  a  straight  line  to  the  left  and  into  the  image  plane 
with  almost  no  rotation.  Due  to  the  uneven  terrain,  the  motion  of  the  camera  is  not  smooth.  The 
trajectories  for  the  feature  points  up  to  the  13'^'"  and  30‘^  frames  are  shown  in  Figure  5.7.  Table  5.3 
lists  the  number  of  feature  points  on  the  tracking  list.  It  is  noted  that,  for  outdoor  images  acquired 
from  a  moving  vehicle,  the  scene  close  to  the  camera  normally  appears  in  the  lower  part  of  the 
images.  A  threshold  is  therefore  set  to  remove  the  detected  points  which  are  far  away  such  as 
points  on  the  cloud.  The  extracted  feature  points  as  well  as  the  new  points  selected  by  the  criterion 
in  Section  5.4  from  the  2“*^,  13***  and  30***  frames  are  also  shown  in  Figure  5.7.  In  this  sequence, 
many  feature  points  move  out  of  the  field  of  view  in  the  first  few  frames.  It  is  therefore  necessary 
to  include  new  feature  points  when  they  become  available.  Also,  it  is  apparent  from  the  sequence 
that  the  vehicle  has  an  abrupt  change  in  heading  direction  at  the  16***  and  20***  frames,  but  the 
algorithm  still  keeps  tracking  most  of  the  feature  points. 

Table  5.3:  The  number  of  feature  points  in  the  tracking  list  for  the  UMASS  Rocket  ALV  Sequence 


frame  number 

1 

2 

3 

4 

5 

6 

7 

8 

9 

10 

11 

12 

13 

14 

15 

tracked  points 

0 

19 

19 

23 

22 

22 

19 

16 

19 

21 

24 

22 

22 

23 

21 

extracted  points 

25 

20 

16 

12 

14 

13 

16 

16 

13 

18 

14 

21 

16 

17 

14 

new  points 

25 

4 

6 

1 

3 

0 

7 

8 

6 

6 

2 

2 

3 

1 

1 

frame  number 

16 

17 

18 

19 

20 

21 

22 

23 

24 

25 

26 

27 

28 

29 

30 

tracked  points 

19 

19 

18 

18 

21 

19 

24 

25 

27 

29 

28 

26 

27 

25 

25 

extracted  points 

19 

19 

19 

18 

22 

15 

17 

16 

18 

18 

17 

16 

19 

19 

18 

new  points 

2 

1 

3 

5 

1 

7 

1 

4 

2 

3 

3 

2 

4 

5 

2 

5.5.4  Martin  Marietta  R3  Sequence 

The  last  sequence  is  one  of  the  four  sequences  distributed  by  Martin  Marietta  as  part  of  the  UGV- 
RSTA  project.  As  in  the  third  sequence,  the  camera  is  mounted  on  a  vehicle  and  the  images  are 
taken  when  the  vehicle  is  moving  through  an  outdoor  environment.  The  original  sequence  consists 
of  densely  sampled  images  of  size  347  x  238;  only  thirty  frames  (every  fifth  frame)  in  the  original 
sequence  were  used  in  the  experiment.  During  the  acquisition  of  the  images,  the  vehicle  moves  to 
the  right  and  slightly  into  the  scene.  Figure  5.8  shows  the  trajectories  of  a  set  of  feature  points  from 
the  first  frame  to  the  19***  and  30***  frames.  As  seen  from  the  figures,  the  points  on  the  mountain 
are  far  away  from  the  vehicle,  resulting  in  small  movements  on  the  image  plane.  Figure  5.8  also 
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shows  the  feature  points  detected  in  the  2“*^,  19‘^  and  frames  and  the  points  added  to  the 
tracking  list.  The  dynamic  inclusion  of  the  new  feature  points  is  summarized  in  Table  5.4. 


Table  5.4:  The  number  of  feature  points  in  the  tracking  list  for  the  Martin  Marietta  R3  Sequence 


frame  number 

1 

2 

3 

4 

5 

6 

7 

8 

9 

10 

11 

12 

13 

14 

15 

tracked  points 

0 

9 

12 

18 

21 

20 

23 

22 

25 

25 

27 

27 

30 

30 

28 

extracted  points 

9 

15 

17 

18 

15 

15 

13 

14 

10 

17 

11 

15 

17 

10 

12 

new  points 

9 

4 

8 

3 

0 

3 

0 

3 

1 

4 

0 

3 

2 

0 

2 

frame  number 

16 

17 

18 

19 

20 

21 

22 

23 

24 

25 

26 

27 

28 

29 

30 

tracked  points 

27 

29 

30 

31 

32 

33 

33 

33 

37 

38 

37 

37 

37 

39 

37 

extracted  points 

18 

19 

18 

41 

15 

17 

15 

17 

19 

19 

20 

15 

16 

10 

17 

new  points 

2 

1 

4 

1 

2 

2 

1 

4 

1 

2 

3 

3 

5 

0 

3 

5.6  Conclusions 

This  chapter  has  presented  an  algorithm  for  tracking  a  dynamic  set  of  feature  points  to  sub-pixel 
accuracy  over  a  sequence  of  images.  To  exploit  the  temporal  information  contained  in  a  sequence, 
a  simple  2D  kinematic  motion  model  is  employed  locally  to  describe  the  trajectory  of  each  feature 
point.  Due  to  the  use  of  a  localized  tracking  scheme,  complicated  3D  motion/structure  estimation 
problems  are  avoided.  To  account  for  non-smooth  changes  in  the  image  motion  arising  from  the 
3D  movements  of  the  camera,  an  inter-frame  motion  estimation  scheme  is  designed.  Therefore, 
the  algorithm  is  able  to  follow  arbitrary  movements  of  feature  points.  Moreover,  a  scheme  which 
is  able  to  include  as  well  as  track  new  points  detected  in  the  subsequent  frames  is  proposed.  The 
scheme  efficiently  preserves  the  information  in  a  sequence,  and  thus  makes  the  algorithm  useful  for 
estimating  the  pose  and  ego-motion  of  the  camera. 
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(a)  Feature  points  in  the  first  frame  (d)  Feature  points  in  the  third  frame 


(b)  Trajectories  up  to  the  19“  frame 


(c)  Trajectories  up  to  the  30***  frame  (f)  Feature  points  in  the  30*^  frame 
Figure  5.5:  Tracking  results  for  the  UMASS  PUMA2  Sequence 
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(a)  Feature  points  in  the  first  frame  (d)  Feature  points  in  the  second  frame 


(b)  Trajectories  up  to  the  tenth  frame  (e)  Feature  points  in  the  tenth  frame 


(c)  Trajectories  up  to  the  15*^  frame  (f)  Feature  points  in  the  15*^  frame 


Figure  5.6:  Tracking  results  for  the  Coke  Can  Sequence 


(c)  Trajectories  up  to  the  30***  frame  (f)  Feature  points  in  the  30*  frame 
Figure  5.7:  Tracking  results  for  the  Rocket  ALV  Sequence 
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(a)  Feature  points  in  the  first  frame  (d)  Feature  points  in  the  second  frame 


(b)  Trajectories  up  to  the  19*^*^  frame 


(c)  Trajectories  up  to  the  frame 


(e)  Feature  points  in  the  19*^  frame 


(f)  Feature  points  in  the  frame 


Figure  5.8:  Tracking  results  for  the  Martin  Marietta  R3  Sequence 


Chapter  6 


Conclusions  and  Future  Work 


6.1  Conclusions 

In  this  dissertation,  several  image  sequence  analysis  problems  important  for  off-road  vehicular 
navigation  have  been  studied. 

We  first  study  the  issue  of  image  stabilization,  i.e.  the  removal  of  unwanted  image  motion 
resulting  from  camera  rotation.  Multiple  visual  cues  are  integrated  and  the  dynamic  nature  of 
a  sequence  is  exploited  using  EKF  in  both  calibrated  and  uncalibrated  stabilization  schemes.  In 
particular,  in  the  calibrated  scheme,  distant  points  and  horizon  fines  are  integrated  by  applying  the 
same  imaging  model  to  the  respective  3D  position  vector  and  normal  vector.  On  the  other  hand, 
for  the  uncafibrated  sequences,  the  imaging  model  cannot  be  applied  to  horizon  fines  detected  from 
the  images.  Alternatively,  the  image  motion  of  horizon  fines  is  characterized  by  the  motion  of  the 
associated  2D  image  plane  normal  vector.  This  results  in  different  projective  transformations  in 
describing  the  image  motion  of  distant  points  and  horizon  fines.  However,  since  the  corresponding 
projective  coefficients  are  not  sensitive  to  the  variations  of  intrinsic  parameters,  only  three  rota¬ 
tional  parameters  need  to  be  estimated  in  the  uncafibrated  scheme,  as  in  the  calibrated  scheme. 
Consequently,  points  and  fines  are  integrated  in  both  stabilization  schemes.  Due  to  the  direct  esti¬ 
mation  of  parameters  relevant  to  stabilization,  the  approach  is  therefore  simple  and  robust.  Most 
importantly,  it  is  suitable  for  subsequent  motion  analysis  since  the  residual  motion  in  the  stabilized 
sequence  has  been  analyzed. 

Subsequently,  we  investigate  the  recovery  of  motion/structure  from  selective  stabilization.  The 
algorithm  first  uses  the  proposed  image  stabilization  scheme  for  the  estimation  of  total  rotation. 
Then,  to  separate  smooth  rotation  and  residual  rotation,  a  kinetic  model  of  the  platform  hosting  the 
camera  is  employed.  By  applying  a  maneuver  detection  scheme  for  the  detection  of  the  beginning 
and  end  of  smooth  rotation,  appropriate  dynamic  laws  are  thereafter  used  to  achieve  selective 
stabilization.  Consequently,  the  structure  parameters  of  a  set  of  close  features  are  estimated  in  a 
less  perturbed  frame  of  reference.  Due  to  the  incorporation  of  kinetic  models,  the  proposed  scheme 
is  therefore  more  closely  related  to  mechanical  stabilization,  and  provides  an  alternative  solution 
to  the  problem. 

Finally,  we  study  the  problem  of  feature  correspondence  over  a  sequence.  A  2D,  localized  fea¬ 
ture  point  tracking  algorithm  is  proposed.  The  algorithm  employs  a  simple,  2D  kinematic  model 
to  describe  the  trajectory  of  a  feature  point.  Feature  correspondences  are  obtained  by  recursively 
applying  the  following  three  steps:  inter-frame  motion  estimation,  corresponding  point  identifica¬ 
tion  and  temporal  information  processing.  The  inter-frame  motion  estimation  step  compensates  for 
the  motion  between  two  windows  likely  to  contain  the  matching  points.  The  step  of  corresponding 
point  identification  consists  of  several  procedures  such  as  correlation  matching  and  interpolation 
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to  identify  the  matching  point  to  snb-pixel  accuracy.  The  last  step  incorporates  the  information 
contained  in  the  current  frame  for  further  tracking.  Due  to  the  localized  operations  of  the  algo¬ 
rithm,  the  scheme  is  able  to  account  for  the  effect  of  3D  camera  motion.  In  addition,  we  also 
consider  the  inclusion  of  new  feature  points  detected  from  subsequent  frames.  An  efficient  scheme 
to  maintain  the  number  of  points  being  tracked  is  designed.  Moreover,  since  each  feature  point  is 
tracked  independently,  parallel  implementations  of  the  proposed  algorithm  are  possible. 


6.2  Suggestions  for  Future  Work 

In  this  final  section,  we  suggest  a  few  extensions  of  this  dissertation. 

•  For  image  stabilization,  we  have  implicitly  assumed  that  distant  features  can  be  differentiated 
from  nearby  features  by  active  sensors  such  as  laser  radars.  This  leads  to  a  sensor  fusion 
problem.  Alternatively,  such  problems  can  be  approached  from  an  image  understanding 
point  of  view.  A  batch  scheme  which  processes  the  first  few  images  may  be  designed  to  select 
desired  features. 

•  Currently,  we  have  tested  the  image  stabilization  algorithm  on  the  images  taken  from  a  camera 
mounted  on  a  moving  vehicle.  Distant  features  are  assumed  to  exist  in  the  images.  However, 
the  algorithm  should  be  tried  on  images  where  the  scene  is  close  to  the  camera.  In  these 
images,  if  there  exist  features  whose  image  motion  is  mainly  due  to  the  rotation,  they  can  be 
used  to  estimate  the  rotational  parameters  for  achieving  image  stabilization. 

•  For  selective  stabilization,  the  kinetic  model  of  a  vehicle  has  been  used  in  this  dissertation  to 
model  the  residual  oscillatory  behavior.  Other  approaches  may  be  employed.  For  example, 
the  parameters  which  best  describe  the  oscillatory  behavior  of  the  host  can  be  estimated  from 
the  image  motion  of  distant  features  in  the  first  few  seconds.  The  resulting  model  can  then 
be  used  for  achieving  selective  stabilization. 

•  Feedback  control  and  mechanical  stabilization.  The  separation  of  oscillatory  rotation  and 
smooth  rotation  provides  useful  information  for  the  design  of  a  suspension  system.  It  can 
also  be  combined  with  inertial  data  measured  from  inertial  sensors  for  achieving  mechanical 
stabilization. 

•  Model-based  compression.  Since  image  stabilization  results  in  a  translation-only  sequence, 
model-based  compression  schemes  can  be  developed.  For  example,  an  affine  transformation 
can  be  used  to  model  the  motion  in  a  stabilized  sequence.  The  scaling  parameter  can  be 
employed  to  account  for  the  looming  component,  while  the  2D  translational  parameters  can 
take  into  account  the  other  two  degrees  of  freedom. 

•  Hardware  implementation.  The  image  stabilization  and  feature  tracking  algorithms  now  run 
on  Sun  SPARC  stations.  Since  many  computationally  expensive  operations  such  as  matrix 
computations  are  required  for  these  schemes,  real-time  implementations  of  the  algorithms 
will  be  challenging  tasks.  Appropriate  modifications  may  be  required  in  order  to  use  im¬ 
age  processing  hardware  such  as  the  Datacube,  or  a  DSP  chip  like  the  C80,  for  real-time 
performance. 
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Appendix  A 


Derivation  of  Image  Motion 


We  give  the  derivation  of  image  motion  in  vector  form  in  this  appendix.  As  in  Section  3.2,  assume 
that  perspective  projection  is  used  as  the  imaging  model  and  the  camera  undergoes  translation  and 
rotation.  Then,  for  a  3D  point  P  :  (X,y,Z)^,  the  image  motion  of  the  projection  point  p  :  {x,yY 
can  be  obtained  as  follows: 
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As  before,  define  pfoe  :  (fc^,  fc^)^  +  pc  and  r  :  ^  as  well  as  the  following  matrices: 
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Then  (A.4)  can  be  rewritten  as 
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Appendix  B 


Representation  of  the  Image  Plane  Normal  Vector 


We  derive  the  relationship  between  w,  the  projection  of  the  3D  normal  vector  of  the  plane  11  (see 
Figure  3.2),  and  the  image  plane  normal  o  in  this  appendix.  Consider  an  image  horizon  line  denoted 

by 

£  :y  =  a  +  bx  (B.l) 

where  (x,  y)^  are  the  image  plane  coordinates  of  a  3D  point.  Then  the  image  plane  vector  normal 
to  £,  defined  in  (3.20),  is 

o  =  (-^V  (B.2) 

a  a 

Let  fc  be  the  focal  length  of  the  camera  and  assume  that  the  optical  axis  intersects  the  image  plane 
at  Pc  :  (xc,  yc)^.  Then 

Pi  :  {0,a  +  bxo-yoJcf  /r> 

P2  :  ^  '  ’ 

are  the  coordinates  of  two  points  lying  on  the  plane  11,  with  respect  to  the  camera  frame  of  reference. 
Consequently,  by  taking  the  cross  product  of  Pi  and  P2,  we  have 


N  =  Pi  X  P2 
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The  3D  normal  vector  W  is  then  obtained  by 

W  = 

l|N|| 

w  is  related  to  o  by 
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Appendix  C 


Derivation  of  Vehicle  Dynamics 


We  apply  Lagrange’s  equations  of  motion  to  derive  the  residual  oscillatory  behavior  of  the  four- 
wheel  vehicle  model  in  this  appendix. 

Consider  Figures  3.3  and  3.4.  Define  the  generalized  coordinates,  consisting  of  seven  degrees  of 
freedom,  as 

q  =  (s-l  >  2^3)  ^7?  (^•^) 

Then  Lagrange’s  equations  of  motion  are  written  as  follows  [16]: 


d  (dT\  -1  7 

dtydqjj  dqj  dqj  dqj  ^  ^ 


(C.2) 


where  {qj,j  =  1, . . . ,  7}  constitute  the  generalized  coordinates,  T  is  the  kinetic  energy  of  the  system, 
U  is  the  potential  energy,  D  is  the  dissipation  function  and  the  (Qj’s  represent  the  generalized 
forces  acting  on  the  system  during  vibration.  In  particular,  as  regards  the  behavior  of  Xc,0,0,  if 
the  suspension  system  is  of  passive  type,  then  the  corresponding  generalized  force  is  assumed  to  be 
zero.  We  look  at  various  energy  terms  in  the  following  paragraphs. 


•  Kinetic  energy 

It  is  well  known  that  the  kinetic  energy  of  a  moving  rigid  body  is  equal  to  the  sum  of  the 
translational  and  rotational  kinetic  energy.  Therefore,  taking  into  account  the  movements  of  the 
unsprung  elements  and  oscillations  of  the  sprung  element,  the  kinetic  energy  is 

T  =  Tb  +  T^  (C.3) 

where  Tb  is  the  kinetic  energy  contributed  by  the  sprung  element 

Tb  =  \MBil  +  (C-4) 

with  Mb  denoting  the  mass  of  the  sprung  element,  and  lyy  and  I^z  are  the  moments  of  inertia  with 
respect  to  the  pitch  and  roll  axes.  on  the  other  hand,  is  the  kinetic  energy  contributed  by  the 
unsprung  elements: 

+  \m^}xI  +  +  \M^ri7  (C.5) 
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•  Potential  energy 

The  potential  energy  TJ  takes  into  account  the  influences  of  suspension  springs  and  tires  on 
a  vehicle  undergoing  vibration.  To  obtain  17,  the  deformations  of  suspension  springs  are  first 
computed.  They  are  approximated  by 

^f,i  ~  -f-  Wj^O  X3  ^  ~ 

dr,r  «  Xc-Ts,r<^-WBff  -X5  ^  ^  ^ 

dr, I  ^  3^0  +  Ts,i4>  -  Wb9  —  X7 

Then  assuming  that  linear  relationships  hold  between  the  deformations  and  the  forces  generated 
by  the  springs,  the  potential  energy  is 


U  =  Us  ^Ut 

where  Us  is  the  energy  stored  in  the  suspension  systems 


and  Ut  is  the  energy  stored  in  the  tires, 
1 


Vs  =  +  ky.i4, + 


Ut  =  ^Kt  [(xi  -  xoi)^  +  (x3  -  XQ2f  +  (3:5  -  3:03)^  +  (3^7  -  3:04)^ 


(C.7) 


(C.8) 


(C.9) 


•  Dissipation  function 

The  dissipation  function  accounts  for  the  effects  of  the  shock  absorbers.  Assuming  that  the 
forces  generated  by  the  shock  absorbers  vary  flnearly  with  the  deforming  rate,  the  dissipation 
function  D  is 

D  =  +  \CrAr  +  \Cr,ldll  (C-IO) 

After  various  energy  terms  have  been  considered,  (C.2)  can  be  applied.  The  residual  oscillatory 
behavior,  especially  6  and  <f>^  of  the  four-wheel  vehicle  model  is  obtained. 
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Appendix  D 


Performance  Analysis  of  Approximate  Kinetic  Laws 


In  this  appendix,  we  analytically  compare  the  performance  of  the  approximate  dynamic  law  of 
pitch  and  roll  motion  in  (4.3)  to  the  more  complete  dynamic  law  in  (3.34).  Equivalently,  assume 
that  the  smooth  rotation  and  the  residual  oscillatory  rotation  are  described  by 

xo  =  Gxo  +  Fu  US  (D.l) 

where  xq  =  {‘^syi^^szi'^sY •>  ®  defined  in  (4.13),  and 

j,  _  02X2  02x10  (D.2) 

04x2  Fus 


with  F„s  mentioned  in  (3.34).  We  study  the  behavior  of  the  following  linear  systems: 


xi  =  Gxi 
oJi  =  Fxi 


(D.3) 


where  Xi  consists  of  the  same  parameters  as  xq.  denotes  the  measurable  total  angular  velocity, 
as  defined  in  (4.12). 

We  employ  a  prediction-correction  based  scheme  to  separate  the  smooth  and  residual  oscillatory 
rotation  [12]: 

ii  =  Gxi-fKi[aJi-Fxi]  (D.4) 


The  resulting  error  xi  =  xq  —  xi  then  satisfies 

Xi  =  (G  —  KiF)xi  -|-  Fuus 


(D.5) 


By  choosing  an  appropriate  Kj,  xi  will  remain  bounded  as  long  as  is  bounded.  Therefore, 
given  reliable  estimates  of  the  total  angular  velocity,  we  can  employ  the  approximate  dynamic  law 
(D.3)  to  achieve  selective  stabilization  without  much  degradation. 
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